From 172a4709e784c25e79e02ff9782303c187d47cc1 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Mon, 7 Dec 2020 16:13:52 -0600 Subject: [PATCH 01/72] Porting vesc meta-package to ROS2. --- vesc/CMakeLists.txt | 6 +++--- vesc/package.xml | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/vesc/CMakeLists.txt b/vesc/CMakeLists.txt index 13b2b81..1caee5d 100644 --- a/vesc/CMakeLists.txt +++ b/vesc/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake_auto REQUIRED) +ament_auto_package() diff --git a/vesc/package.xml b/vesc/package.xml index e1deeb4..d6d68f1 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -14,13 +14,13 @@ Michael T. Boulet Joshua Whitley - catkin + ament_cmake_auto vesc_driver vesc_msgs vesc_ackermann - + ament_cmake From 7a16e51c12bb706559bc68b0e34fe694cf053a3f Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Tue, 8 Dec 2020 10:59:14 -0600 Subject: [PATCH 02/72] Porting all package.xml and CMakeList.txt files to ROS2. --- vesc_ackermann/CMakeLists.txt | 106 +++++++++------------------------- vesc_ackermann/package.xml | 16 ++--- vesc_driver/CMakeLists.txt | 106 +++++++++------------------------- vesc_driver/package.xml | 15 ++--- vesc_msgs/CMakeLists.txt | 30 +++++----- vesc_msgs/package.xml | 15 ++++- 6 files changed, 98 insertions(+), 190 deletions(-) diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt index 166d201..2568d46 100644 --- a/vesc_ackermann/CMakeLists.txt +++ b/vesc_ackermann/CMakeLists.txt @@ -1,101 +1,51 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc_ackermann) -# Setting C++ standard to 11 +# Setting C++ standard to 14 if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_STANDARD 11) + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_STANDARD 14) endif() -find_package(catkin REQUIRED COMPONENTS - ackermann_msgs - geometry_msgs - nav_msgs - nodelet - pluginlib - roscpp - roslint - std_msgs - tf - vesc_msgs -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - ackermann_msgs - geometry_msgs - nav_msgs - nodelet - pluginlib - std_msgs - tf - vesc_msgs -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() ########### ## Build ## ########### -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - # node executable -add_executable(vesc_to_odom_node src/vesc_to_odom_node.cpp - src/vesc_to_odom.cpp) -add_dependencies(vesc_to_odom_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_to_odom_node - ${catkin_LIBRARIES} -) - -add_executable(ackermann_to_vesc_node src/ackermann_to_vesc_node.cpp - src/ackermann_to_vesc.cpp) -add_dependencies(ackermann_to_vesc_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ackermann_to_vesc_node - ${catkin_LIBRARIES} -) - -# nodelet library -add_library(vesc_ackermann_nodelet src/ackermann_to_vesc_nodelet.cpp - src/ackermann_to_vesc.cpp - src/vesc_to_odom_nodelet.cpp - src/vesc_to_odom.cpp) -add_dependencies(vesc_ackermann_nodelet ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_ackermann_nodelet - ${catkin_LIBRARIES} +ament_auto_add_executable(vesc_to_odom_node + src/vesc_to_odom_node.cpp + src/vesc_to_odom.cpp ) -set(ROSLINT_CPP_OPTS "--filter=-build/c++11") -roslint_cpp() - -############# -## Install ## -############# - -install(TARGETS vesc_to_odom_node ackermann_to_vesc_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +ament_auto_add_executable(ackermann_to_vesc_node + src/ackermann_to_vesc_node.cpp + src/ackermann_to_vesc.cpp ) -install(TARGETS vesc_ackermann_nodelet - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# components library +ament_auto_add_library(vesc_ackermann_nodelet + src/ackermann_to_vesc_nodelet.cpp + src/ackermann_to_vesc.cpp + src/vesc_to_odom_nodelet.cpp + src/vesc_to_odom.cpp ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES vesc_ackermann_nodelet.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - roslint_add_test() +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index 34d1301..ef60194 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -14,21 +14,21 @@ Michael T. Boulet Joshua Whitley - catkin - - roslint + ament_cmake_auto ackermann_msgs geometry_msgs nav_msgs - nodelet - pluginlib - roscpp + rclcpp + rclcpp_components std_msgs - tf + tf2 vesc_msgs + ament_lint_auto + ament_lint_common + - + ament_cmake diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 50221d3..3e28c21 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,100 +1,50 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc_driver) -# Set minimum C++ standard to C++11 +# Set minimum C++ standard to C++14 if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++11") - set(CMAKE_CXX_STANDARD 11) + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_STANDARD 14) endif() -find_package(Boost REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - nodelet - pluginlib - roscpp - roslint - serial - std_msgs - vesc_msgs -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - nodelet - pluginlib - std_msgs - vesc_msgs serial -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() ########### ## Build ## ########### -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - # node executable -add_executable(vesc_driver_node src/vesc_driver_node.cpp - src/vesc_driver.cpp - src/vesc_interface.cpp - src/vesc_packet.cpp - src/vesc_packet_factory.cpp) -add_dependencies(vesc_driver_node ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_driver_node - ${catkin_LIBRARIES} +ament_auto_add_executable(vesc_driver_node + src/vesc_driver_node.cpp + src/vesc_driver.cpp + src/vesc_interface.cpp + src/vesc_packet.cpp + src/vesc_packet_factory.cpp ) # nodelet library -add_library(vesc_driver_nodelet src/vesc_driver_nodelet.cpp - src/vesc_driver.cpp - src/vesc_interface.cpp - src/vesc_packet.cpp - src/vesc_packet_factory.cpp) -add_dependencies(vesc_driver_nodelet ${catkin_EXPORTED_TARGETS}) -target_link_libraries(vesc_driver_nodelet - ${catkin_LIBRARIES} -) - -file(GLOB_RECURSE ${PROJECT_NAME}_CPP_FILES - RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} - . *.cpp *.h) -list(REMOVE_ITEM ${PROJECT_NAME}_CPP_FILES - include/${PROJECT_NAME}/crc.h) - -set(ROSLINT_CPP_OPTS "--filter=-build/c++11") -roslint_cpp(${${PROJECT_NAME}_CPP_FILES}) - -############# -## Install ## -############# - -install(TARGETS vesc_driver_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +ament_auto_add_library(vesc_driver_nodelet + src/vesc_driver_nodelet.cpp + src/vesc_driver.cpp + src/vesc_interface.cpp + src/vesc_packet.cpp + src/vesc_packet_factory.cpp ) -install(TARGETS vesc_driver_nodelet - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) -install(FILES vesc_driver_nodelet.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) - ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - roslint_add_test() +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 8b58cc4..6194639 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -14,17 +14,18 @@ Michael T. Boulet Joshua Whitley - catkin - roslint + ament_cmake_auto - nodelet - pluginlib - roscpp - serial + rclcpp + rclcpp_components + serial_driver std_msgs vesc_msgs + ament_lint_auto + ament_lint_common + - + ament_cmake diff --git a/vesc_msgs/CMakeLists.txt b/vesc_msgs/CMakeLists.txt index d1365bd..8b222b0 100644 --- a/vesc_msgs/CMakeLists.txt +++ b/vesc_msgs/CMakeLists.txt @@ -1,22 +1,20 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vesc_msgs) -find_package(catkin REQUIRED COMPONENTS - std_msgs - message_generation -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -add_message_files( - DIRECTORY msg - FILES - VescState.msg - VescStateStamped.msg -) -generate_messages( +rosidl_generate_interfaces(vesc_msgs + "msg/VescState.msg" + "msg/VescStateStamped.msg" DEPENDENCIES - std_msgs + builtin_interfaces + std_msgs ) -catkin_package( - CATKIN_DEPENDS std_msgs message_runtime -) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/vesc_msgs/package.xml b/vesc_msgs/package.xml index dc14307..db7960a 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -14,10 +14,19 @@ Michael T. Boulet Joshua Whitley - catkin - message_generation + ament_cmake_auto + rosidl_default_generators + builtin_interfaces std_msgs + rosidl_default_runtime - message_runtime + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + From af9c27d0a68db72837d1edcc523edf7e9091d493 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Fri, 11 Dec 2020 20:29:09 -0600 Subject: [PATCH 03/72] [vesc_msgs] Ported to ROS2. --- vesc_msgs/msg/VescStateStamped.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vesc_msgs/msg/VescStateStamped.msg b/vesc_msgs/msg/VescStateStamped.msg index 086d5b8..bc1aec9 100644 --- a/vesc_msgs/msg/VescStateStamped.msg +++ b/vesc_msgs/msg/VescStateStamped.msg @@ -1,4 +1,4 @@ # Timestamped VESC open source motor controller state (telemetry) -Header header -VescState state \ No newline at end of file +std_msgs/Header header +VescState state From cff7f0c09a44a2288a4dfe4cfe64debf1d707833 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sat, 12 Dec 2020 08:18:27 -0600 Subject: [PATCH 04/72] [vesc_driver] Removing nodelet XML and renaming header files. --- vesc_driver/include/vesc_driver/{crc.h => crc.hpp} | 0 .../include/vesc_driver/{datatypes.h => datatypes.hpp} | 0 .../include/vesc_driver/{vesc_driver.h => vesc_driver.hpp} | 0 .../vesc_driver/{vesc_interface.h => vesc_interface.hpp} | 0 .../include/vesc_driver/{vesc_packet.h => vesc_packet.hpp} | 0 .../{vesc_packet_factory.h => vesc_packet_factory.hpp} | 0 vesc_driver/vesc_driver_nodelet.xml | 5 ----- 7 files changed, 5 deletions(-) rename vesc_driver/include/vesc_driver/{crc.h => crc.hpp} (100%) rename vesc_driver/include/vesc_driver/{datatypes.h => datatypes.hpp} (100%) rename vesc_driver/include/vesc_driver/{vesc_driver.h => vesc_driver.hpp} (100%) rename vesc_driver/include/vesc_driver/{vesc_interface.h => vesc_interface.hpp} (100%) rename vesc_driver/include/vesc_driver/{vesc_packet.h => vesc_packet.hpp} (100%) rename vesc_driver/include/vesc_driver/{vesc_packet_factory.h => vesc_packet_factory.hpp} (100%) delete mode 100644 vesc_driver/vesc_driver_nodelet.xml diff --git a/vesc_driver/include/vesc_driver/crc.h b/vesc_driver/include/vesc_driver/crc.hpp similarity index 100% rename from vesc_driver/include/vesc_driver/crc.h rename to vesc_driver/include/vesc_driver/crc.hpp diff --git a/vesc_driver/include/vesc_driver/datatypes.h b/vesc_driver/include/vesc_driver/datatypes.hpp similarity index 100% rename from vesc_driver/include/vesc_driver/datatypes.h rename to vesc_driver/include/vesc_driver/datatypes.hpp diff --git a/vesc_driver/include/vesc_driver/vesc_driver.h b/vesc_driver/include/vesc_driver/vesc_driver.hpp similarity index 100% rename from vesc_driver/include/vesc_driver/vesc_driver.h rename to vesc_driver/include/vesc_driver/vesc_driver.hpp diff --git a/vesc_driver/include/vesc_driver/vesc_interface.h b/vesc_driver/include/vesc_driver/vesc_interface.hpp similarity index 100% rename from vesc_driver/include/vesc_driver/vesc_interface.h rename to vesc_driver/include/vesc_driver/vesc_interface.hpp diff --git a/vesc_driver/include/vesc_driver/vesc_packet.h b/vesc_driver/include/vesc_driver/vesc_packet.hpp similarity index 100% rename from vesc_driver/include/vesc_driver/vesc_packet.h rename to vesc_driver/include/vesc_driver/vesc_packet.hpp diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.h b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp similarity index 100% rename from vesc_driver/include/vesc_driver/vesc_packet_factory.h rename to vesc_driver/include/vesc_driver/vesc_packet_factory.hpp diff --git a/vesc_driver/vesc_driver_nodelet.xml b/vesc_driver/vesc_driver_nodelet.xml deleted file mode 100644 index 5660e73..0000000 --- a/vesc_driver/vesc_driver_nodelet.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Vedder VESC motor controller interface nodelet. - - From 300419e4c480ddaf63b3483ee2cd0140b1f4b6d7 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sat, 12 Dec 2020 08:30:39 -0600 Subject: [PATCH 05/72] [vesc_driver] Removing unnecessary files and renaming header files. --- vesc_driver/CMakeLists.txt | 12 +--- .../include/vesc_driver/vesc_driver.hpp | 32 ++++++----- .../include/vesc_driver/vesc_interface.hpp | 10 ++-- .../include/vesc_driver/vesc_packet.hpp | 8 +-- .../vesc_driver/vesc_packet_factory.hpp | 10 ++-- vesc_driver/src/vesc_driver.cpp | 4 +- vesc_driver/src/vesc_driver_node.cpp | 41 -------------- vesc_driver/src/vesc_driver_nodelet.cpp | 56 ------------------- vesc_driver/src/vesc_interface.cpp | 7 +-- vesc_driver/src/vesc_packet.cpp | 7 +-- vesc_driver/src/vesc_packet_factory.cpp | 5 +- 11 files changed, 41 insertions(+), 151 deletions(-) delete mode 100644 vesc_driver/src/vesc_driver_node.cpp delete mode 100644 vesc_driver/src/vesc_driver_nodelet.cpp diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 3e28c21..556a795 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -18,17 +18,7 @@ ament_auto_find_build_dependencies() ########### # node executable -ament_auto_add_executable(vesc_driver_node - src/vesc_driver_node.cpp - src/vesc_driver.cpp - src/vesc_interface.cpp - src/vesc_packet.cpp - src/vesc_packet_factory.cpp -) - -# nodelet library -ament_auto_add_library(vesc_driver_nodelet - src/vesc_driver_nodelet.cpp +ament_auto_add_library(${PROJECT_NAME}_node SHARED src/vesc_driver.cpp src/vesc_interface.cpp src/vesc_packet.cpp diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index bdc5ffd..bf47bdd 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -25,22 +25,24 @@ // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_DRIVER_VESC_DRIVER_H_ -#define VESC_DRIVER_VESC_DRIVER_H_ +#ifndef VESC_DRIVER__VESC_DRIVER_HPP_ +#define VESC_DRIVER__VESC_DRIVER_HPP_ + +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet.hpp" #include #include -#include -#include +#include +#include #include -#include "vesc_driver/vesc_interface.h" -#include "vesc_driver/vesc_packet.h" - namespace vesc_driver { +using std_msgs::msg::Float64; + class VescDriver { public: @@ -96,15 +98,15 @@ class VescDriver int fw_version_minor_; ///< firmware minor version reported by vesc // ROS callbacks - void timerCallback(const ros::TimerEvent& event); - void dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle); - void currentCallback(const std_msgs::Float64::ConstPtr& current); - void brakeCallback(const std_msgs::Float64::ConstPtr& brake); - void speedCallback(const std_msgs::Float64::ConstPtr& speed); - void positionCallback(const std_msgs::Float64::ConstPtr& position); - void servoCallback(const std_msgs::Float64::ConstPtr& servo); + void timerCallback(const rclcpp::TimerEvent& event); + void dutyCycleCallback(const Float64::ConstPtr& duty_cycle); + void currentCallback(const Float64::ConstPtr& current); + void brakeCallback(const Float64::ConstPtr& brake); + void speedCallback(const Float64::ConstPtr& speed); + void positionCallback(const Float64::ConstPtr& position); + void servoCallback(const Float64::ConstPtr& servo); }; } // namespace vesc_driver -#endif // VESC_DRIVER_VESC_DRIVER_H_ +#endif // VESC_DRIVER__VESC_DRIVER_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_interface.hpp b/vesc_driver/include/vesc_driver/vesc_interface.hpp index 558ec2b..21b41d9 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.hpp +++ b/vesc_driver/include/vesc_driver/vesc_interface.hpp @@ -25,8 +25,10 @@ // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_DRIVER_VESC_INTERFACE_H_ -#define VESC_DRIVER_VESC_INTERFACE_H_ +#ifndef VESC_DRIVER__VESC_INTERFACE_HPP_ +#define VESC_DRIVER__VESC_INTERFACE_HPP_ + +#include "vesc_driver/vesc_packet.hpp" #include #include @@ -35,8 +37,6 @@ #include #include -#include "vesc_driver/vesc_packet.h" - namespace vesc_driver { @@ -148,4 +148,4 @@ class SerialException : public std::exception } // namespace vesc_driver -#endif // VESC_DRIVER_VESC_INTERFACE_H_ +#endif // VESC_DRIVER__VESC_INTERFACE_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index 307e2f5..c671a64 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -25,8 +25,8 @@ // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_DRIVER_VESC_PACKET_H_ -#define VESC_DRIVER_VESC_PACKET_H_ +#ifndef VESC_DRIVER__VESC_PACKET_HPP_ +#define VESC_DRIVER__VESC_PACKET_HPP_ #include #include @@ -35,7 +35,7 @@ #include #define CRCPP_USE_CPP11 -#include "vesc_driver/crc.h" +#include "vesc_driver/crc.hpp" namespace vesc_driver { @@ -219,4 +219,4 @@ class VescPacketSetServoPos : public VescPacket } // namespace vesc_driver -#endif // VESC_DRIVER_VESC_PACKET_H_ +#endif // VESC_DRIVER__VESC_PACKET_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp index 643d6f9..377538a 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp @@ -25,8 +25,10 @@ // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_DRIVER_VESC_PACKET_FACTORY_H_ -#define VESC_DRIVER_VESC_PACKET_FACTORY_H_ +#ifndef VESC_DRIVER__VESC_PACKET_FACTORY_HPP_ +#define VESC_DRIVER__VESC_PACKET_FACTORY_HPP_ + +#include "vesc_driver/vesc_packet.hpp" #include #include @@ -35,8 +37,6 @@ #include #include -#include "vesc_driver/vesc_packet.h" - namespace vesc_driver { @@ -111,4 +111,4 @@ static PacketFactoryTemplate global_##klass##Factory((id)); } // namespace vesc_driver -#endif // VESC_DRIVER_VESC_PACKET_FACTORY_H_ +#endif // VESC_DRIVER__VESC_PACKET_FACTORY_HPP_ diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 95371eb..b843518 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -25,7 +25,7 @@ // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_driver.h" +#include "vesc_driver/vesc_driver.hpp" #include #include @@ -33,7 +33,7 @@ #include #include -#include +#include namespace vesc_driver { diff --git a/vesc_driver/src/vesc_driver_node.cpp b/vesc_driver/src/vesc_driver_node.cpp deleted file mode 100644 index 2c61547..0000000 --- a/vesc_driver/src/vesc_driver_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include - -#include "vesc_driver/vesc_driver.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "vesc_driver_node"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - vesc_driver::VescDriver vesc_driver(nh, private_nh); - - ros::spin(); - - return 0; -} diff --git a/vesc_driver/src/vesc_driver_nodelet.cpp b/vesc_driver/src/vesc_driver_nodelet.cpp deleted file mode 100644 index 34134af..0000000 --- a/vesc_driver/src/vesc_driver_nodelet.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -#include - -#include "vesc_driver/vesc_driver.h" - -namespace vesc_driver -{ - -class VescDriverNodelet: public nodelet::Nodelet -{ -public: - VescDriverNodelet() {} - -private: - virtual void onInit(void); - - std::shared_ptr vesc_driver_; -}; // class VescDriverNodelet - -void VescDriverNodelet::onInit() -{ - NODELET_DEBUG("Initializing VESC driver nodelet"); - vesc_driver_.reset(new VescDriver(getNodeHandle(), getPrivateNodeHandle())); -} - -} // namespace vesc_driver - -PLUGINLIB_EXPORT_CLASS(vesc_driver::VescDriverNodelet, nodelet::Nodelet); diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 3febe1a..0aea0fc 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -25,7 +25,8 @@ // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_interface.h" +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" #include @@ -36,10 +37,6 @@ #include #include -#include - -#include "vesc_driver/vesc_packet_factory.h" - namespace vesc_driver { diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 8e96365..f7555b1 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -25,16 +25,15 @@ // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_packet.h" +#include "vesc_driver/datatypes.hpp" +#include "vesc_driver/vesc_packet.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" #include #include #include #include -#include "vesc_driver/vesc_packet_factory.h" -#include "vesc_driver/datatypes.h" - namespace vesc_driver { diff --git a/vesc_driver/src/vesc_packet_factory.cpp b/vesc_driver/src/vesc_packet_factory.cpp index 328401e..4fbbf36 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -25,15 +25,14 @@ // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/vesc_packet_factory.h" +#include "vesc_driver/vesc_packet.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" #include #include #include #include -#include "vesc_driver/vesc_packet.h" - namespace vesc_driver { From da23406c2449e0f815585ef2b5cc9c30cdb6f042 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sat, 12 Dec 2020 09:25:22 -0600 Subject: [PATCH 06/72] [vesc_driver] Starting port of vesc_driver. --- .../include/vesc_driver/vesc_driver.hpp | 53 ++++--- vesc_driver/src/vesc_driver.cpp | 142 +++++++++++------- 2 files changed, 116 insertions(+), 79 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index bf47bdd..46b5981 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -34,38 +34,43 @@ #include #include +#include #include +#include #include -#include +#include namespace vesc_driver { using std_msgs::msg::Float64; +using vesc_msgs::msg::VescState; class VescDriver +: public autoware::drivers::serial_driver::SerialDriverNode { public: - VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh); + VescDriver(const rclcpp::NodeOptions & options); private: // interface to the VESC VescInterface vesc_; - void vescPacketCallback(const std::shared_ptr& packet); - void vescErrorCallback(const std::string& error); + void vescPacketCallback(const std::shared_ptr & packet); + void vescErrorCallback(const std::string & error); // limits on VESC commands struct CommandLimit { - CommandLimit(const ros::NodeHandle& nh, const std::string& str, - const boost::optional& min_lower = boost::optional(), - const boost::optional& max_upper = boost::optional()); + CommandLimit( + const std::string& str, + const boost::optional& min_lower = boost::optional(), + const boost::optional& max_upper = boost::optional()); double clip(double value); std::string name; boost::optional lower; boost::optional upper; }; + CommandLimit duty_cycle_limit_; CommandLimit current_limit_; CommandLimit brake_limit_; @@ -74,15 +79,15 @@ class VescDriver CommandLimit servo_limit_; // ROS services - ros::Publisher state_pub_; - ros::Publisher servo_sensor_pub_; - ros::Subscriber duty_cycle_sub_; - ros::Subscriber current_sub_; - ros::Subscriber brake_sub_; - ros::Subscriber speed_sub_; - ros::Subscriber position_sub_; - ros::Subscriber servo_sub_; - ros::Timer timer_; + rclcpp::PublisherBase::SharedPtr state_pub_; + rclcpp::PublisherBase::SharedPtr servo_sensor_pub_; + rclcpp::SubscriptionBase::SharedPtr duty_cycle_sub_; + rclcpp::SubscriptionBase::SharedPtr current_sub_; + rclcpp::SubscriptionBase::SharedPtr brake_sub_; + rclcpp::SubscriptionBase::SharedPtr speed_sub_; + rclcpp::SubscriptionBase::SharedPtr position_sub_; + rclcpp::SubscriptionBase::SharedPtr servo_sub_; + rclcpp::TimerBase::SharedPtr timer_; // driver modes (possible states) typedef enum @@ -98,13 +103,13 @@ class VescDriver int fw_version_minor_; ///< firmware minor version reported by vesc // ROS callbacks - void timerCallback(const rclcpp::TimerEvent& event); - void dutyCycleCallback(const Float64::ConstPtr& duty_cycle); - void currentCallback(const Float64::ConstPtr& current); - void brakeCallback(const Float64::ConstPtr& brake); - void speedCallback(const Float64::ConstPtr& speed); - void positionCallback(const Float64::ConstPtr& position); - void servoCallback(const Float64::ConstPtr& servo); + void brakeCallback(const Float64::SharedPtr & brake); + void currentCallback(const Float64::SharedPtr & current); + void dutyCycleCallback(const Float64::SharedPtr & duty_cycle); + void positionCallback(const Float64::SharedPtr & position); + void servoCallback(const Float64::SharedPtr & servo); + void speedCallback(const Float64::SharedPtr & speed); + void timerCallback(); }; } // namespace vesc_driver diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index b843518..8cbae43 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -28,20 +28,28 @@ #include "vesc_driver/vesc_driver.hpp" #include +#include #include #include #include #include +#include #include namespace vesc_driver { +using autoware::drivers::serial_driver::SerialDriverNode; +using namespace std::chrono_literals; using std::placeholders::_1; +using std_msgs::msg::Float64; +using vesc_msgs::msg::VescStateStamped; -VescDriver::VescDriver(ros::NodeHandle nh, - ros::NodeHandle private_nh) : +VescDriver::VescDriver(const rclcpp::NodeOptions & options) : + SerialDriverNode( + "vesc_comms", + options), vesc_(std::string(), std::bind(&VescDriver::vescPacketCallback, this, _1), std::bind(&VescDriver::vescErrorCallback, this, _1)), @@ -54,8 +62,8 @@ VescDriver::VescDriver(ros::NodeHandle nh, std::string port; if (!private_nh.getParam("port", port)) { - ROS_FATAL("VESC communication port parameter required."); - ros::shutdown(); + RCLCPP_FATAL(get_logger(), "VESC communication port parameter required."); + rclcpp::shutdown(); return; } @@ -66,29 +74,35 @@ VescDriver::VescDriver(ros::NodeHandle nh, } catch (SerialException e) { - ROS_FATAL("Failed to connect to the VESC, %s.", e.what()); - ros::shutdown(); + RCLCPP_FATAL(get_logger(), "Failed to connect to the VESC, %s.", e.what()); + rclcpp::shutdown(); return; } // create vesc state (telemetry) publisher - state_pub_ = nh.advertise("sensors/core", 10); + state_pub_ = create_publisher("sensors/core", rclcpp::QoS{10}); // since vesc state does not include the servo position, publish the commanded // servo position as a "sensor" - servo_sensor_pub_ = nh.advertise("sensors/servo_position_command", 10); + servo_sensor_pub_ = create_publisher( + "sensors/servo_position_command", rclcpp::QoS{10}); // subscribe to motor and servo command topics - duty_cycle_sub_ = nh.subscribe("commands/motor/duty_cycle", 10, - &VescDriver::dutyCycleCallback, this); - current_sub_ = nh.subscribe("commands/motor/current", 10, &VescDriver::currentCallback, this); - brake_sub_ = nh.subscribe("commands/motor/brake", 10, &VescDriver::brakeCallback, this); - speed_sub_ = nh.subscribe("commands/motor/speed", 10, &VescDriver::speedCallback, this); - position_sub_ = nh.subscribe("commands/motor/position", 10, &VescDriver::positionCallback, this); - servo_sub_ = nh.subscribe("commands/servo/position", 10, &VescDriver::servoCallback, this); + duty_cycle_sub_ = create_subscription( + "commands/motor/duty_cycle", rclcpp::QoS{10}, std::bind(&VescDriver::dutyCycleCallback, this, _1)); + current_sub_ = create_subscription( + "commands/motor/current", rclcpp::QoS{10}, std::bind(&VescDriver::currentCallback, this, _1)); + brake_sub_ = create_subscription( + "commands/motor/brake", rclcpp::QoS{10}, std::bind(&VescDriver::brakeCallback, this, _1)); + speed_sub_ = create_subscription( + "commands/motor/speed", rclcpp::QoS{10}, std::bind(&VescDriver::speedCallback, this, _1)); + position_sub_ = create_subscription( + "commands/motor/position", rclcpp::QoS{10}, std::bind(&VescDriver::positionCallback, this, _1)); + servo_sub_ = create_subscription( + "commands/servo/position", rclcpp::QoS{10}, std::bind(&VescDriver::servoCallback, this, _1)); // create a 50Hz timer, used for state machine & polling VESC telemetry - timer_ = nh.createTimer(ros::Duration(1.0 / 50.0), &VescDriver::timerCallback, this); + timer_ = create_wall_timer(20ms, std::bind(&VescDriver::timerCallback, this, _1)); } /* TODO or TO-THINKABOUT LIST @@ -104,14 +118,14 @@ VescDriver::VescDriver(ros::NodeHandle nh, - try to predict vesc bounds (from vesc config) and command detect bounds errors */ -void VescDriver::timerCallback(const ros::TimerEvent& event) +void VescDriver::timerCallback() { // VESC interface should not unexpectedly disconnect, but test for it anyway if (!vesc_.isConnected()) { - ROS_FATAL("Unexpectedly disconnected from serial port."); + RCLCPP_FATAL(get_logger(), "Unexpectedly disconnected from serial port."); timer_.stop(); - ros::shutdown(); + rclcpp::shutdown(); return; } @@ -126,8 +140,9 @@ void VescDriver::timerCallback(const ros::TimerEvent& event) 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_); + RCLCPP_INFO( + get_logger(), "Connected to VESC with firmware version %d.%d", + fw_version_major_, fw_version_minor_); driver_mode_ = MODE_OPERATING; } } @@ -143,15 +158,15 @@ void VescDriver::timerCallback(const ros::TimerEvent& event) } } -void VescDriver::vescPacketCallback(const std::shared_ptr& packet) +void VescDriver::vescPacketCallback(const std::shared_ptr & packet) { if (packet->name() == "Values") { std::shared_ptr values = std::dynamic_pointer_cast(packet); - vesc_msgs::VescStateStamped::Ptr state_msg(new vesc_msgs::VescStateStamped); - state_msg->header.stamp = ros::Time::now(); + VescStateStamped::SharedPtr state_msg(new VescStateStamped); + state_msg->header.stamp = now(); state_msg->state.voltage_input = values->v_in(); state_msg->state.temperature_pcb = values->temp_pcb(); state_msg->state.current_motor = values->current_motor(); @@ -178,9 +193,9 @@ void VescDriver::vescPacketCallback(const std::shared_ptr& pac } } -void VescDriver::vescErrorCallback(const std::string& error) +void VescDriver::vescErrorCallback(const std::string & error) { - ROS_ERROR("%s", error.c_str()); + RCLCPP_ERROR(get_logger(), "%s", error.c_str()); } /** @@ -188,7 +203,7 @@ void VescDriver::vescErrorCallback(const std::string& error) * note that the VESC may impose a more restrictive bounds on the range depending * on its configuration, e.g. absolute value is between 0.05 and 0.95. */ -void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle) +void VescDriver::dutyCycleCallback(const Float64::SharedPtr duty_cycle) { if (driver_mode_ = MODE_OPERATING) { @@ -201,7 +216,7 @@ void VescDriver::dutyCycleCallback(const std_msgs::Float64::ConstPtr& duty_cycle * note that the VESC may impose a more restrictive bounds on the range depending on * its configuration. */ -void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) +void VescDriver::currentCallback(const Float64::SharedPtr current) { if (driver_mode_ = MODE_OPERATING) { @@ -214,7 +229,7 @@ void VescDriver::currentCallback(const std_msgs::Float64::ConstPtr& current) * However, note that the VESC may impose a more restrictive bounds on the range * depending on its configuration. */ -void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) +void VescDriver::brakeCallback(const Float64::SharedPtr brake) { if (driver_mode_ = MODE_OPERATING) { @@ -228,7 +243,7 @@ void VescDriver::brakeCallback(const std_msgs::Float64::ConstPtr& brake) * driver. However, note that the VESC may impose a more restrictive bounds on the * range depending on its configuration. */ -void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) +void VescDriver::speedCallback(const Float64::SharedPtr speed) { if (driver_mode_ = MODE_OPERATING) { @@ -240,7 +255,7 @@ void VescDriver::speedCallback(const std_msgs::Float64::ConstPtr& speed) * @param position Commanded VESC motor position in radians. Any value is accepted by this driver. * Note that the VESC must be in encoder mode for this command to have an effect. */ -void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) +void VescDriver::positionCallback(const Float64::SharedPtr position) { if (driver_mode_ = MODE_OPERATING) { @@ -253,7 +268,7 @@ void VescDriver::positionCallback(const std_msgs::Float64::ConstPtr& position) /** * @param servo Commanded VESC servo output position. Valid range is 0 to 1. */ -void VescDriver::servoCallback(const std_msgs::Float64::ConstPtr& servo) +void VescDriver::servoCallback(const Float64::SharedPtr servo) { if (driver_mode_ = MODE_OPERATING) { @@ -266,10 +281,11 @@ 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) +VescDriver::CommandLimit::CommandLimit( + const std::string & str, + const boost::optional& min_lower, + 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 +294,16 @@ 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 << ")."); + RCLCPP_WARN_STREAM( + get_logger(), "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 << ")."); + RCLCPP_WARN_STREAM( + get_logger(), "Parameter " << name << "_min (" << param_min << + ") is greater than the feasible maximum (" << *max_upper << ")."); } else { @@ -304,14 +322,16 @@ 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 << ")."); + RCLCPP_WARN_STREAM( + get_logger(), "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 << ")."); + RCLCPP_WARN_STREAM( + get_logger(), "Parameter " << name << "_max (" << param_max << + ") is greater than the feasible maximum (" << *max_upper << ")."); } else { @@ -326,8 +346,9 @@ 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 << ")."); + RCLCPP_WARN_STREAM( + get_logger(), "Parameter " << name << "_max (" << *upper << + ") is less than parameter " << name << "_min (" << *lower << ")."); double temp(*lower); lower = *upper; upper = temp; @@ -335,25 +356,36 @@ VescDriver::CommandLimit::CommandLimit(const ros::NodeHandle& nh, const std::str std::ostringstream oss; oss << " " << name << " limit: "; - if (lower) oss << *lower << " "; - else oss << "(none) "; - if (upper) oss << *upper; - else oss << "(none)"; - ROS_DEBUG_STREAM(oss.str()); + + if (lower) { + oss << *lower << " "; + } else { + oss << "(none) "; + } + + if (upper) { + oss << *upper; + } else { + oss << "(none)"; + } + + RCLCPP_DEBUG_STREAM(get_logger(), oss.str()); } double VescDriver::CommandLimit::clip(double value) { if (lower && value < lower) { - ROS_INFO_THROTTLE(10, "%s command value (%f) below minimum limit (%f), clipping.", - name.c_str(), value, *lower); + RCLCPP_INFO_THROTTLE( + get_logger(), 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); + RCLCPP_INFO_THROTTLE( + get_logger(), 10, "%s command value (%f) above maximum limit (%f), clipping.", + name.c_str(), value, *upper); return *upper; } return value; From 1e0a01fb8b297c8133450c29a4a22c022b5c2f21 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sat, 12 Dec 2020 11:04:41 -0600 Subject: [PATCH 07/72] [vesc_driver] Working on implementation of rclcpp::Node. --- vesc_driver/CMakeLists.txt | 1 + .../include/vesc_driver/vesc_driver.hpp | 26 ++-- vesc_driver/package.xml | 3 +- vesc_driver/param/default.params.yaml | 7 + vesc_driver/src/vesc_driver.cpp | 136 +++++++++--------- 5 files changed, 94 insertions(+), 79 deletions(-) create mode 100644 vesc_driver/param/default.params.yaml diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 556a795..a2adf41 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + param ) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index 46b5981..e1205df 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -36,18 +36,19 @@ #include #include -#include #include #include +#include namespace vesc_driver { using std_msgs::msg::Float64; using vesc_msgs::msg::VescState; +using vesc_msgs::msg::VescStateStamped; class VescDriver -: public autoware::drivers::serial_driver::SerialDriverNode +: public rclcpp::Node { public: VescDriver(const rclcpp::NodeOptions & options); @@ -62,10 +63,13 @@ class VescDriver struct CommandLimit { CommandLimit( - const std::string& str, + rclcpp::Node * node_ptr, + const std::string & str, const boost::optional& min_lower = boost::optional(), const boost::optional& max_upper = boost::optional()); double clip(double value); + rclcpp::Node * node_ptr; + rclcpp::Logger logger; std::string name; boost::optional lower; boost::optional upper; @@ -79,8 +83,8 @@ class VescDriver CommandLimit servo_limit_; // ROS services - rclcpp::PublisherBase::SharedPtr state_pub_; - rclcpp::PublisherBase::SharedPtr servo_sensor_pub_; + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Publisher::SharedPtr servo_sensor_pub_; rclcpp::SubscriptionBase::SharedPtr duty_cycle_sub_; rclcpp::SubscriptionBase::SharedPtr current_sub_; rclcpp::SubscriptionBase::SharedPtr brake_sub_; @@ -103,12 +107,12 @@ class VescDriver int fw_version_minor_; ///< firmware minor version reported by vesc // ROS callbacks - void brakeCallback(const Float64::SharedPtr & brake); - void currentCallback(const Float64::SharedPtr & current); - void dutyCycleCallback(const Float64::SharedPtr & duty_cycle); - void positionCallback(const Float64::SharedPtr & position); - void servoCallback(const Float64::SharedPtr & servo); - void speedCallback(const Float64::SharedPtr & speed); + void brakeCallback(const Float64::SharedPtr brake); + void currentCallback(const Float64::SharedPtr current); + void dutyCycleCallback(const Float64::SharedPtr duty_cycle); + void positionCallback(const Float64::SharedPtr position); + void servoCallback(const Float64::SharedPtr servo); + void speedCallback(const Float64::SharedPtr speed); void timerCallback(); }; diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 6194639..c956327 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -16,9 +16,10 @@ ament_cmake_auto + libboost-dev + libboost-system-dev rclcpp rclcpp_components - serial_driver std_msgs vesc_msgs diff --git a/vesc_driver/param/default.params.yaml b/vesc_driver/param/default.params.yaml new file mode 100644 index 0000000..ed5bfb2 --- /dev/null +++ b/vesc_driver/param/default.params.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + device_name: "/dev/ttyACM0" + baud_rate: 115200 + flow_control: "none" + parity: "none" + stop_bits: "1" diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 8cbae43..516c095 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -40,40 +40,34 @@ namespace vesc_driver { -using autoware::drivers::serial_driver::SerialDriverNode; using namespace std::chrono_literals; using std::placeholders::_1; using std_msgs::msg::Float64; using vesc_msgs::msg::VescStateStamped; -VescDriver::VescDriver(const rclcpp::NodeOptions & options) : - SerialDriverNode( - "vesc_comms", - options), - 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(const rclcpp::NodeOptions & options) + : rclcpp::Node("vesc_driver", options), + vesc_( + std::string(), + std::bind(&VescDriver::vescPacketCallback, this, _1), + std::bind(&VescDriver::vescErrorCallback, this, _1)), + duty_cycle_limit_(this, "duty_cycle", -1.0, 1.0), + current_limit_(this, "current"), + brake_limit_(this, "brake"), + speed_limit_(this, "speed"), + position_limit_(this, "position"), + servo_limit_(this, "servo", 0.0, 1.0), + driver_mode_(MODE_INITIALIZING), + fw_version_major_(-1), + fw_version_minor_(-1) { // get vesc serial port address - std::string port; - if (!private_nh.getParam("port", port)) - { - RCLCPP_FATAL(get_logger(), "VESC communication port parameter required."); - rclcpp::shutdown(); - return; - } + std::string port = declare_parameter("port", ""); // attempt to connect to the serial port - try - { + try { vesc_.connect(port); - } - catch (SerialException e) - { + } catch (SerialException e) { RCLCPP_FATAL(get_logger(), "Failed to connect to the VESC, %s.", e.what()); rclcpp::shutdown(); return; @@ -102,7 +96,7 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) : "commands/servo/position", rclcpp::QoS{10}, std::bind(&VescDriver::servoCallback, this, _1)); // create a 50Hz timer, used for state machine & polling VESC telemetry - timer_ = create_wall_timer(20ms, std::bind(&VescDriver::timerCallback, this, _1)); + timer_ = create_wall_timer(20ms, std::bind(&VescDriver::timerCallback, this)); } /* TODO or TO-THINKABOUT LIST @@ -124,7 +118,6 @@ void VescDriver::timerCallback() if (!vesc_.isConnected()) { RCLCPP_FATAL(get_logger(), "Unexpectedly disconnected from serial port."); - timer_.stop(); rclcpp::shutdown(); return; } @@ -141,8 +134,8 @@ void VescDriver::timerCallback() if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) { RCLCPP_INFO( - get_logger(), "Connected to VESC with firmware version %d.%d", - fw_version_major_, fw_version_minor_); + get_logger(), "Connected to VESC with firmware version %d.%d", + fw_version_major_, fw_version_minor_); driver_mode_ = MODE_OPERATING; } } @@ -165,23 +158,23 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa std::shared_ptr values = std::dynamic_pointer_cast(packet); - VescStateStamped::SharedPtr state_msg(new VescStateStamped); - state_msg->header.stamp = now(); - state_msg->state.voltage_input = values->v_in(); - state_msg->state.temperature_pcb = values->temp_pcb(); - state_msg->state.current_motor = values->current_motor(); - state_msg->state.current_input = values->current_in(); - state_msg->state.speed = values->rpm(); - state_msg->state.duty_cycle = values->duty_now(); - state_msg->state.charge_drawn = values->amp_hours(); - state_msg->state.charge_regen = values->amp_hours_charged(); - state_msg->state.energy_drawn = values->watt_hours(); - state_msg->state.energy_regen = values->watt_hours_charged(); - state_msg->state.displacement = values->tachometer(); - state_msg->state.distance_traveled = values->tachometer_abs(); - state_msg->state.fault_code = values->fault_code(); - - state_pub_.publish(state_msg); + auto state_msg = VescStateStamped(); + state_msg.header.stamp = now(); + state_msg.state.voltage_input = values->v_in(); + state_msg.state.temperature_pcb = values->temp_pcb(); + state_msg.state.current_motor = values->current_motor(); + state_msg.state.current_input = values->current_in(); + state_msg.state.speed = values->rpm(); + state_msg.state.duty_cycle = values->duty_now(); + state_msg.state.charge_drawn = values->amp_hours(); + state_msg.state.charge_regen = values->amp_hours_charged(); + state_msg.state.energy_drawn = values->watt_hours(); + state_msg.state.energy_regen = values->watt_hours_charged(); + state_msg.state.displacement = values->tachometer(); + state_msg.state.distance_traveled = values->tachometer_abs(); + state_msg.state.fault_code = values->fault_code(); + + state_pub_->publish(state_msg); } else if (packet->name() == "FWVersion") { @@ -275,39 +268,44 @@ void VescDriver::servoCallback(const Float64::SharedPtr servo) double servo_clipped(servo_limit_.clip(servo->data)); vesc_.setServo(servo_clipped); // publish clipped servo value as a "sensor" - std_msgs::Float64::Ptr servo_sensor_msg(new std_msgs::Float64); - servo_sensor_msg->data = servo_clipped; - servo_sensor_pub_.publish(servo_sensor_msg); + auto servo_sensor_msg = Float64(); + servo_sensor_msg.data = servo_clipped; + servo_sensor_pub_->publish(servo_sensor_msg); } } VescDriver::CommandLimit::CommandLimit( + rclcpp::Node * node_ptr, const std::string & str, const boost::optional& min_lower, const boost::optional& max_upper) - : name(str) + : node_ptr(node_ptr), + logger(node_ptr->get_logger()), + name(str) { // check if user's minimum value is outside of the range min_lower to max_upper - double param_min; - if (nh.getParam(name + "_min", param_min)) + auto param_min = + node_ptr->declare_parameter(name + "_min", rclcpp::ParameterValue(0.0)); + + if (param_min.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { - if (min_lower && param_min < *min_lower) + if (min_lower && param_min.get() < *min_lower) { lower = *min_lower; RCLCPP_WARN_STREAM( - get_logger(), "Parameter " << name << "_min (" << param_min << + logger, "Parameter " << name << "_min (" << param_min.get() << ") is less than the feasible minimum (" << *min_lower << ")."); } - else if (max_upper && param_min > *max_upper) + else if (max_upper && param_min.get() > *max_upper) { lower = *max_upper; RCLCPP_WARN_STREAM( - get_logger(), "Parameter " << name << "_min (" << param_min << + logger, "Parameter " << name << "_min (" << param_min.get() << ") is greater than the feasible maximum (" << *max_upper << ")."); } else { - lower = param_min; + lower = param_min.get(); } } else if (min_lower) @@ -316,26 +314,28 @@ VescDriver::CommandLimit::CommandLimit( } // check if the uers' maximum value is outside of the range min_lower to max_upper - double param_max; - if (nh.getParam(name + "_max", param_max)) + auto param_max = + node_ptr->declare_parameter(name + "_max", rclcpp::ParameterValue(0.0)); + + if (param_max.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { - if (min_lower && param_max < *min_lower) + if (min_lower && param_max.get() < *min_lower) { upper = *min_lower; RCLCPP_WARN_STREAM( - get_logger(), "Parameter " << name << "_max (" << param_max << + logger, "Parameter " << name << "_max (" << param_max.get() << ") is less than the feasible minimum (" << *min_lower << ")."); } - else if (max_upper && param_max > *max_upper) + else if (max_upper && param_max.get() > *max_upper) { upper = *max_upper; RCLCPP_WARN_STREAM( - get_logger(), "Parameter " << name << "_max (" << param_max << + logger, "Parameter " << name << "_max (" << param_max.get() << ") is greater than the feasible maximum (" << *max_upper << ")."); } else { - upper = param_max; + upper = param_max.get(); } } else if (max_upper) @@ -347,7 +347,7 @@ VescDriver::CommandLimit::CommandLimit( if (upper && lower && *lower > *upper) { RCLCPP_WARN_STREAM( - get_logger(), "Parameter " << name << "_max (" << *upper << + logger, "Parameter " << name << "_max (" << *upper << ") is less than parameter " << name << "_min (" << *lower << ")."); double temp(*lower); lower = *upper; @@ -369,22 +369,24 @@ VescDriver::CommandLimit::CommandLimit( oss << "(none)"; } - RCLCPP_DEBUG_STREAM(get_logger(), oss.str()); + RCLCPP_DEBUG_STREAM(logger, oss.str()); } double VescDriver::CommandLimit::clip(double value) { + auto clock = rclcpp::Clock(RCL_ROS_TIME); + if (lower && value < lower) { RCLCPP_INFO_THROTTLE( - get_logger(), 10, "%s command value (%f) below minimum limit (%f), clipping.", + logger, clock, 10, "%s command value (%f) below minimum limit (%f), clipping.", name.c_str(), value, *lower); return *lower; } if (upper && value > upper) { RCLCPP_INFO_THROTTLE( - get_logger(), 10, "%s command value (%f) above maximum limit (%f), clipping.", + logger, clock, 10, "%s command value (%f) above maximum limit (%f), clipping.", name.c_str(), value, *upper); return *upper; } From 730e606cc4707cb3ce702610a4bb02aa413e5a23 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 15:45:06 -0600 Subject: [PATCH 08/72] [vesc_driver] Finished port - applied uncrustify. --- vesc_driver/include/vesc_driver/crc.hpp | 935 +++++++++--------- .../include/vesc_driver/vesc_driver.hpp | 6 +- .../include/vesc_driver/vesc_interface.hpp | 29 +- .../include/vesc_driver/vesc_packet.hpp | 13 +- .../vesc_driver/vesc_packet_factory.hpp | 15 +- vesc_driver/src/vesc_driver.cpp | 113 +-- vesc_driver/src/vesc_interface.cpp | 133 +-- vesc_driver/src/vesc_packet.cpp | 110 +-- vesc_driver/src/vesc_packet_factory.cpp | 78 +- 9 files changed, 732 insertions(+), 700 deletions(-) diff --git a/vesc_driver/include/vesc_driver/crc.hpp b/vesc_driver/include/vesc_driver/crc.hpp index 68e322d..336f7e2 100644 --- a/vesc_driver/include/vesc_driver/crc.hpp +++ b/vesc_driver/include/vesc_driver/crc.hpp @@ -52,7 +52,7 @@ multiplication in the bit-by-bit calculation instead of a small conditional. The branchless implementation may be faster on processor architectures which support single-instruction integer multiplication. #define CRCPP_USE_CPP11 - Define to enables C++11 features (move semantics, constexpr, static_assert, etc.). - #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. + #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. */ #ifndef VESC_DRIVER_CRC_H_ @@ -71,59 +71,59 @@ #ifndef crcpp_uint8 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. # define crcpp_uint8 ::std::uint8_t # else - /// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 8-bit integer definition, used primarily for parameter definitions. # define crcpp_uint8 uint8_t # endif #endif #ifndef crcpp_uint16 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. # define crcpp_uint16 ::std::uint16_t # else - /// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 16-bit integer definition, used primarily for parameter definitions. # define crcpp_uint16 uint16_t # endif #endif #ifndef crcpp_uint32 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. # define crcpp_uint32 ::std::uint32_t # else - /// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 32-bit integer definition, used primarily for parameter definitions. # define crcpp_uint32 uint32_t # endif #endif #ifndef crcpp_uint64 # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. # define crcpp_uint64 ::std::uint64_t # else - /// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. +/// @brief Unsigned 64-bit integer definition, used primarily for parameter definitions. # define crcpp_uint64 uint64_t # endif #endif #ifndef crcpp_size # ifdef CRCPP_USE_CPP11 - /// @brief Unsigned size definition, used for specifying data sizes. +/// @brief Unsigned size definition, used for specifying data sizes. # define crcpp_size ::std::size_t # else - /// @brief Unsigned size definition, used for specifying data sizes. +/// @brief Unsigned size definition, used for specifying data sizes. # define crcpp_size size_t # endif #endif #ifdef CRCPP_USE_CPP11 - /// @brief Compile-time expression definition. +/// @brief Compile-time expression definition. # define crcpp_constexpr constexpr #else - /// @brief Compile-time expression definition. +/// @brief Compile-time expression definition. # define crcpp_constexpr const #endif @@ -142,163 +142,175 @@ namespace CRCPP class CRC { public: - // Forward declaration - template - struct Table; - - /** - @brief CRC parameters. - */ - template - struct Parameters - { - CRCType polynomial; ///< CRC polynomial - CRCType initialValue; ///< Initial CRC value - CRCType finalXOR; ///< Value to XOR with the final CRC - bool reflectInput; ///< true to reflect all input bytes - bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) - - Table MakeTable() const; - }; - - /** - @brief CRC lookup table. After construction, the CRC parameters are fixed. - @note A CRC table can be used for multiple CRC calculations. - */ - template - struct Table - { - // Constructors are intentionally NOT marked explicit. - Table(const Parameters & parameters); + // Forward declaration + template + struct Table; + + /** + @brief CRC parameters. + */ + template + struct Parameters + { + CRCType polynomial; ///< CRC polynomial + CRCType initialValue; ///< Initial CRC value + CRCType finalXOR; ///< Value to XOR with the final CRC + bool reflectInput; ///< true to reflect all input bytes + bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) + + 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); + Table(Parameters && parameters); #endif - const Parameters & GetParameters() const; + const Parameters & GetParameters() const; - const CRCType * GetTable() const; + const CRCType * GetTable() const; - CRCType operator[](unsigned char index) 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) +private: + void InitTable(); + + Parameters parameters; ///< CRC parameters used to construct the table + CRCType table[1 << CHAR_BIT]; ///< CRC lookup table + }; + + // The number of bits in CRCType must be at least as large as CRCWidth. + // CRCType must be an unsigned integer type or a custom type with operator overloads. + template + static CRCType Calculate( + const void * data, crcpp_size size, const Parameters & parameters); + + template + static CRCType Calculate( + const void * data, crcpp_size size, const Parameters & parameters, CRCType crc); + + template + static CRCType Calculate( + const void * data, crcpp_size size, const Table & lookupTable); + + template + static CRCType Calculate( + const void * data, crcpp_size size, const Table & lookupTable, CRCType crc); + + // Common CRCs up to 64 bits. + // Note: Check values are the computed CRCs when given an ASCII input of "123456789" (without null terminator) #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 4> & CRC_4_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_EPC(); - static const Parameters< crcpp_uint8, 5> & CRC_5_ITU(); - static const Parameters< crcpp_uint8, 5> & CRC_5_USB(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000A(); - static const Parameters< crcpp_uint8, 6> & CRC_6_CDMA2000B(); - static const Parameters< crcpp_uint8, 6> & CRC_6_ITU(); - static const Parameters< crcpp_uint8, 7> & CRC_7(); + static const Parameters & CRC_4_ITU(); + static const Parameters & CRC_5_EPC(); + static const Parameters & CRC_5_ITU(); + static const Parameters & CRC_5_USB(); + static const Parameters & CRC_6_CDMA2000A(); + static const Parameters & CRC_6_CDMA2000B(); + static const Parameters & CRC_6_ITU(); + static const Parameters & CRC_7(); #endif - static const Parameters< crcpp_uint8, 8> & CRC_8(); + static const Parameters & CRC_8(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters< crcpp_uint8, 8> & CRC_8_EBU(); - static const Parameters< crcpp_uint8, 8> & CRC_8_MAXIM(); - static const Parameters< crcpp_uint8, 8> & CRC_8_WCDMA(); - static const Parameters & CRC_10(); - static const Parameters & CRC_10_CDMA2000(); - static const Parameters & CRC_11(); - static const Parameters & CRC_12_CDMA2000(); - static const Parameters & CRC_12_DECT(); - static const Parameters & CRC_12_UMTS(); - static const Parameters & CRC_13_BBC(); - static const Parameters & CRC_15(); - static const Parameters & CRC_15_MPT1327(); + static const Parameters & CRC_8_EBU(); + static const Parameters & CRC_8_MAXIM(); + static const Parameters & CRC_8_WCDMA(); + static const Parameters & CRC_10(); + static const Parameters & CRC_10_CDMA2000(); + static const Parameters & CRC_11(); + static const Parameters & CRC_12_CDMA2000(); + static const Parameters & CRC_12_DECT(); + static const Parameters & CRC_12_UMTS(); + static const Parameters & CRC_13_BBC(); + static const Parameters & CRC_15(); + static const Parameters & CRC_15_MPT1327(); #endif - static const Parameters & CRC_16_ARC(); - static const Parameters & CRC_16_BUYPASS(); - static const Parameters & CRC_16_CCITTFALSE(); + static const Parameters & CRC_16_ARC(); + static const Parameters & CRC_16_BUYPASS(); + static const Parameters & CRC_16_CCITTFALSE(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_CDMA2000(); - static const Parameters & CRC_16_CMS(); - static const Parameters & CRC_16_DECTR(); - static const Parameters & CRC_16_DECTX(); - static const Parameters & CRC_16_DNP(); + static const Parameters & CRC_16_CDMA2000(); + static const Parameters & CRC_16_CMS(); + static const Parameters & CRC_16_DECTR(); + static const Parameters & CRC_16_DECTX(); + static const Parameters & CRC_16_DNP(); #endif - static const Parameters & CRC_16_GENIBUS(); - static const Parameters & CRC_16_KERMIT(); + static const Parameters & CRC_16_GENIBUS(); + static const Parameters & CRC_16_KERMIT(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_16_MAXIM(); - static const Parameters & CRC_16_MODBUS(); - static const Parameters & CRC_16_T10DIF(); - static const Parameters & CRC_16_USB(); + static const Parameters & CRC_16_MAXIM(); + static const Parameters & CRC_16_MODBUS(); + static const Parameters & CRC_16_T10DIF(); + static const Parameters & CRC_16_USB(); #endif - static const Parameters & CRC_16_X25(); - static const Parameters & CRC_16_XMODEM(); + static const Parameters & CRC_16_X25(); + static const Parameters & CRC_16_XMODEM(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_17_CAN(); - static const Parameters & CRC_21_CAN(); - static const Parameters & CRC_24(); - static const Parameters & CRC_24_FLEXRAYA(); - static const Parameters & CRC_24_FLEXRAYB(); - static const Parameters & CRC_30(); + static const Parameters & CRC_17_CAN(); + static const Parameters & CRC_21_CAN(); + static const Parameters & CRC_24(); + static const Parameters & CRC_24_FLEXRAYA(); + static const Parameters & CRC_24_FLEXRAYB(); + static const Parameters & CRC_30(); #endif - static const Parameters & CRC_32(); - static const Parameters & CRC_32_BZIP2(); + static const Parameters & CRC_32(); + static const Parameters & CRC_32_BZIP2(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_C(); + static const Parameters & CRC_32_C(); #endif - static const Parameters & CRC_32_MPEG2(); - static const Parameters & CRC_32_POSIX(); + static const Parameters & CRC_32_MPEG2(); + static const Parameters & CRC_32_POSIX(); #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - static const Parameters & CRC_32_Q(); - static const Parameters & CRC_40_GSM(); - static const Parameters & CRC_64(); + static const Parameters & CRC_32_Q(); + static const Parameters & CRC_40_GSM(); + static const Parameters & CRC_64(); #endif #ifdef CRCPP_USE_CPP11 - CRC() = delete; - CRC(const CRC & other) = delete; - CRC & operator=(const CRC & other) = delete; - CRC(CRC && other) = delete; - CRC & operator=(CRC && other) = delete; + CRC() = delete; + CRC(const CRC & other) = delete; + CRC & operator=(const CRC & other) = delete; + CRC(CRC && other) = delete; + CRC & operator=(CRC && other) = delete; #endif private: #ifndef CRCPP_USE_CPP11 - CRC(); - CRC(const CRC & other); - CRC & operator=(const CRC & other); + CRC(); + CRC(const CRC & other); + CRC & operator=(const CRC & other); #endif - template - static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); + template + static IntegerType Reflect(IntegerType value, crcpp_uint16 numBits); - template - static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + template + static CRCType Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - template - static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); + template + static CRCType UndoFinalize(CRCType remainder, CRCType finalXOR, bool reflectOutput); - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); + template + static CRCType CalculateRemainder( + const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder); - template - static CRCType CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); + template + static CRCType CalculateRemainder( + const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder); }; /** @@ -309,11 +321,11 @@ class CRC @tparam CRCWidth Number of bits in the CRC @return CRC lookup table */ -template +template inline CRC::Table CRC::Parameters::MakeTable() const { - // This should take advantage of RVO and optimize out the copy. - return CRC::Table(*this); + // This should take advantage of RVO and optimize out the copy. + return CRC::Table(*this); } /** @@ -322,11 +334,11 @@ inline CRC::Table CRC::Parameters::MakeTab @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC */ -template -inline CRC::Table::Table(const Parameters & params) : - parameters(params) +template +inline CRC::Table::Table(const Parameters & params) +: parameters(params) { - InitTable(); + InitTable(); } #ifdef CRCPP_USE_CPP11 @@ -336,11 +348,11 @@ inline CRC::Table::Table(const Parameters @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC */ -template -inline CRC::Table::Table(Parameters && params) : - parameters(::std::move(params)) +template +inline CRC::Table::Table(Parameters && params) +: parameters(::std::move(params)) { - InitTable(); + InitTable(); } #endif @@ -350,10 +362,11 @@ inline CRC::Table::Table(Parameters && par @tparam CRCWidth Number of bits in the CRC @return CRC parameters */ -template -inline const CRC::Parameters & CRC::Table::GetParameters() const +template +inline const CRC::Parameters & CRC::Table::GetParameters() const { - return parameters; + return parameters; } /** @@ -362,10 +375,10 @@ inline const CRC::Parameters & CRC::Table: @tparam CRCWidth Number of bits in the CRC @return CRC table */ -template +template inline const CRCType * CRC::Table::GetTable() const { - return table; + return table; } /** @@ -375,10 +388,10 @@ inline const CRCType * CRC::Table::GetTable() const @tparam CRCWidth Number of bits in the CRC @return CRC table entry */ -template +template inline CRCType CRC::Table::operator[](unsigned char index) const { - return table[index]; + return table[index]; } /** @@ -386,39 +399,37 @@ inline CRCType CRC::Table::operator[](unsigned char index) co @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC */ -template +template inline void CRC::Table::InitTable() { - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); - - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK((CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1))); - CRCType crc; - unsigned char byte = 0; + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - // Loop over each dividend (each possible number storable in an unsigned char) - do - { - crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); + CRCType crc; + unsigned char byte = 0; - // This mask might not be necessary; all unit tests pass with this line commented out, - // but that might just be a coincidence based on the CRC parameters used for testing. - // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. - crc &= BIT_MASK; + // Loop over each dividend (each possible number storable in an unsigned char) + do { + crc = CRC::CalculateRemainder(&byte, sizeof(byte), parameters, CRCType(0)); - if (!parameters.reflectInput && CRCWidth < CHAR_BIT) - { - // Undo the special operation at the end of the CalculateRemainder() - // function for non-reflected CRCs < CHAR_BIT. - crc = static_cast(crc << SHIFT); - } + // This mask might not be necessary; all unit tests pass with this line commented out, + // but that might just be a coincidence based on the CRC parameters used for testing. + // In any case, this is harmless to leave in and only adds a single machine instruction per loop iteration. + crc &= BIT_MASK; - table[byte] = crc; + if (!parameters.reflectInput && CRCWidth < CHAR_BIT) { + // Undo the special operation at the end of the CalculateRemainder() + // function for non-reflected CRCs < CHAR_BIT. + crc = static_cast(crc << SHIFT); } - while (++byte); + + table[byte] = crc; + } while (++byte); } /** @@ -430,14 +441,18 @@ inline void CRC::Table::InitTable() @tparam CRCWidth Number of bits in the CRC @return CRC */ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Parameters & parameters) { - CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); + CRCType remainder = CalculateRemainder(data, size, parameters, parameters.initialValue); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @brief Appends additional data to a previous CRC calculation. @@ -450,16 +465,23 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Paramete @tparam CRCWidth Number of bits in the CRC @return CRC */ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Parameters & parameters, CRCType crc) { - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + CRCType remainder = UndoFinalize( + crc, parameters.finalXOR, + parameters.reflectInput != + parameters.reflectOutput); - remainder = CalculateRemainder(data, size, parameters, remainder); + remainder = CalculateRemainder(data, size, parameters, remainder); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @@ -471,16 +493,20 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Paramete @tparam CRCWidth Number of bits in the CRC @return CRC */ -template -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Table & lookupTable) { - const Parameters & parameters = lookupTable.GetParameters(); + const Parameters & parameters = lookupTable.GetParameters(); - CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); + CRCType remainder = CalculateRemainder(data, size, lookupTable, parameters.initialValue); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @@ -494,18 +520,25 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table -inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) +template +inline CRCType CRC::Calculate( + const void * data, crcpp_size size, const Table & lookupTable, CRCType crc) { - const Parameters & parameters = lookupTable.GetParameters(); + const Parameters & parameters = lookupTable.GetParameters(); - CRCType remainder = UndoFinalize(crc, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + CRCType remainder = UndoFinalize( + crc, parameters.finalXOR, + parameters.reflectInput != + parameters.reflectOutput); - remainder = CalculateRemainder(data, size, lookupTable, remainder); + remainder = CalculateRemainder(data, size, lookupTable, remainder); - // No need to mask the remainder here; the mask will be applied in the Finalize() function. + // No need to mask the remainder here; the mask will be applied in the Finalize() function. - return Finalize(remainder, parameters.finalXOR, parameters.reflectInput != parameters.reflectOutput); + return Finalize( + remainder, parameters.finalXOR, + parameters.reflectInput != parameters.reflectOutput); } /** @@ -515,18 +548,17 @@ inline CRCType CRC::Calculate(const void * data, crcpp_size size, const Table +template inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) { - IntegerType reversedValue(0); + IntegerType reversedValue(0); - for (crcpp_uint16 i = 0; i < numBits; ++i) - { - reversedValue = static_cast((reversedValue << 1) | (value & 1)); - value = static_cast(value >> 1); - } + for (crcpp_uint16 i = 0; i < numBits; ++i) { + reversedValue = static_cast((reversedValue << 1) | (value & 1)); + value = static_cast(value >> 1); + } - return reversedValue; + return reversedValue; } /** @@ -538,19 +570,18 @@ inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) @tparam CRCWidth Number of bits in the CRC @return Final CRC */ -template +template inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOutput) { - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - if (reflectOutput) - { - remainder = Reflect(remainder, CRCWidth); - } + if (reflectOutput) { + remainder = Reflect(remainder, CRCWidth); + } - return (remainder ^ finalXOR) & BIT_MASK; + return (remainder ^ finalXOR) & BIT_MASK; } /** @@ -570,21 +601,20 @@ inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOu @tparam CRCWidth Number of bits in the CRC @return Un-finalized CRC remainder */ -template +template inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutput) { - // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) - static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | - ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); + // For masking off the bits for the CRC (in the event that the number of bits in CRCType is larger than CRCWidth) + static crcpp_constexpr CRCType BIT_MASK = (CRCType(1) << (CRCWidth - CRCType(1))) | + ((CRCType(1) << (CRCWidth - CRCType(1))) - CRCType(1)); - crc = (crc & BIT_MASK) ^ finalXOR; + crc = (crc & BIT_MASK) ^ finalXOR; - if (reflectOutput) - { - crc = Reflect(crc, CRCWidth); - } + if (reflectOutput) { + crc = Reflect(crc, CRCWidth); + } - return crc; + return crc; } /** @@ -597,111 +627,117 @@ inline CRCType CRC::UndoFinalize(CRCType crc, CRCType finalXOR, bool reflectOutp @tparam CRCWidth Number of bits in the CRC @return CRC remainder */ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) +template +inline CRCType CRC::CalculateRemainder( + const void * data, crcpp_size size, const Parameters & parameters, CRCType remainder) { #ifdef CRCPP_USE_CPP11 - // This static_assert is put here because this function will always be compiled in no matter what - // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. - static_assert(::std::numeric_limits::digits >= CRCWidth, "CRCType is too small to contain a CRC of width CRCWidth."); + // This static_assert is put here because this function will always be compiled in no matter what + // the template parameters are and whether or not a table lookup or bit-by-bit algorithm is used. + static_assert( + ::std::numeric_limits::digits >= CRCWidth, + "CRCType is too small to contain a CRC of width CRCWidth."); #else - // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's - // better than nothing. - enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; + // Catching this compile-time error is very important. Sadly, the compiler error will be very cryptic, but it's + // better than nothing. + enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / + (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; #endif - const unsigned char * current = reinterpret_cast(data); + const unsigned char * current = reinterpret_cast(data); - // Slightly different implementations based on the parameters. The current implementations try to eliminate as much - // computation from the inner loop (looping over each bit) as possible. - if (parameters.reflectInput) - { - CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); - while (size--) - { - remainder = static_cast(remainder ^ *current++); + // Slightly different implementations based on the parameters. The current implementations try to eliminate as much + // computation from the inner loop (looping over each bit) as possible. + if (parameters.reflectInput) { + CRCType polynomial = CRC::Reflect(parameters.polynomial, CRCWidth); + while (size--) { + remainder = static_cast(remainder ^ *current++); - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) { #ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & 1) - // remainder = (remainder >> 1) ^ polynomial; - // else - // remainder >>= 1; - remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & 1) + // remainder = (remainder >> 1) ^ polynomial; + // else + // remainder >>= 1; + remainder = static_cast((remainder >> 1) ^ ((remainder & 1) * polynomial)); #else - remainder = static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> 1)); + remainder = + static_cast((remainder & 1) ? ((remainder >> 1) ^ polynomial) : (remainder >> + 1)); #endif - } - } + } } - else if (CRCWidth >= CHAR_BIT) - { - static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); + } else if (CRCWidth >= CHAR_BIT) { + static crcpp_constexpr CRCType CRC_WIDTH_MINUS_ONE(CRCWidth - CRCType(1)); #ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); + static crcpp_constexpr CRCType CRC_HIGHEST_BIT_MASK(CRCType(1) << CRC_WIDTH_MINUS_ONE); #endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - while (size--) - { - remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); + while (size--) { + remainder = static_cast(remainder ^ (static_cast(*current++) << SHIFT)); - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) { #ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CRC_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ parameters.polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CRC_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ parameters.polynomial; + // else + // remainder <<= 1; + remainder = + static_cast((remainder << + 1) ^ (((remainder >> CRC_WIDTH_MINUS_ONE) & 1) * parameters.polynomial)); #else - remainder = static_cast((remainder & CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); + remainder = + static_cast((remainder & + CRC_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ parameters.polynomial) : (remainder << 1)); #endif - } - } + } } - else - { - static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); + } else { + static crcpp_constexpr CRCType CHAR_BIT_MINUS_ONE(CHAR_BIT - 1); #ifndef CRCPP_BRANCHLESS - static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); + static crcpp_constexpr CRCType CHAR_BIT_HIGHEST_BIT_MASK(CRCType(1) << CHAR_BIT_MINUS_ONE); #endif - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - CRCType polynomial = static_cast(parameters.polynomial << SHIFT); - remainder = static_cast(remainder << SHIFT); + CRCType polynomial = static_cast(parameters.polynomial << SHIFT); + remainder = static_cast(remainder << SHIFT); - while (size--) - { - remainder = static_cast(remainder ^ *current++); + while (size--) { + remainder = static_cast(remainder ^ *current++); - // An optimizing compiler might choose to unroll this loop. - for (crcpp_size i = 0; i < CHAR_BIT; ++i) - { + // An optimizing compiler might choose to unroll this loop. + for (crcpp_size i = 0; i < CHAR_BIT; ++i) { #ifdef CRCPP_BRANCHLESS - // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: - // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) - // remainder = (remainder << 1) ^ polynomial; - // else - // remainder <<= 1; - remainder = static_cast((remainder << 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); + // Clever way to avoid a branch at the expense of a multiplication. This code is equivalent to the following: + // if (remainder & CHAR_BIT_HIGHEST_BIT_MASK) + // remainder = (remainder << 1) ^ polynomial; + // else + // remainder <<= 1; + remainder = + static_cast((remainder << + 1) ^ (((remainder >> CHAR_BIT_MINUS_ONE) & 1) * polynomial)); #else - remainder = static_cast((remainder & CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); + remainder = + static_cast((remainder & + CHAR_BIT_HIGHEST_BIT_MASK) ? ((remainder << 1) ^ polynomial) : (remainder << 1)); #endif - } - } - - remainder = static_cast(remainder >> SHIFT); + } } - return remainder; + remainder = static_cast(remainder >> SHIFT); + } + + return remainder; } /** @@ -714,55 +750,55 @@ inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const @tparam CRCWidth Number of bits in the CRC @return CRC remainder */ -template -inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) +template +inline CRCType CRC::CalculateRemainder( + const void * data, crcpp_size size, const Table & lookupTable, CRCType remainder) { - const unsigned char * current = reinterpret_cast(data); + const unsigned char * current = reinterpret_cast(data); - if (lookupTable.GetParameters().reflectInput) - { - while (size--) - { + if (lookupTable.GetParameters().reflectInput) { + while (size--) { #if defined(WIN32) || defined(_WIN32) || defined(WINCE) - // Disable warning about data loss when doing (remainder >> CHAR_BIT) when - // remainder is one byte long. The algorithm is still correct in this case, - // though it's possible that one additional machine instruction will be executed. + // Disable warning about data loss when doing (remainder >> CHAR_BIT) when + // remainder is one byte long. The algorithm is still correct in this case, + // though it's possible that one additional machine instruction will be executed. # pragma warning (push) # pragma warning (disable : 4333) #endif - remainder = static_cast((remainder >> CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); + remainder = + static_cast((remainder >> + CHAR_BIT) ^ lookupTable[static_cast(remainder ^ *current++)]); #if defined(WIN32) || defined(_WIN32) || defined(WINCE) # pragma warning (pop) #endif - } } - else if (CRCWidth >= CHAR_BIT) - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - - while (size--) - { - remainder = static_cast((remainder << CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]); - } - } - else - { - // The conditional expression is used to avoid a -Wshift-count-overflow warning. - static crcpp_constexpr CRCType SHIFT((CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); + } else if (CRCWidth >= CHAR_BIT) { + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CRCWidth >= CHAR_BIT) ? static_cast(CRCWidth - CHAR_BIT) : 0); - remainder = static_cast(remainder << SHIFT); + while (size--) { + remainder = + static_cast((remainder << + CHAR_BIT) ^ lookupTable[static_cast((remainder >> SHIFT) ^ *current++)]); + } + } else { + // The conditional expression is used to avoid a -Wshift-count-overflow warning. + static crcpp_constexpr CRCType SHIFT( + (CHAR_BIT >= CRCWidth) ? static_cast(CHAR_BIT - CRCWidth) : 0); - while (size--) - { - // Note: no need to mask here since remainder is guaranteed to fit in a single byte. - remainder = lookupTable[static_cast(remainder ^ *current++)]; - } + remainder = static_cast(remainder << SHIFT); - remainder = static_cast(remainder >> SHIFT); + while (size--) { + // Note: no need to mask here since remainder is guaranteed to fit in a single byte. + remainder = lookupTable[static_cast(remainder ^ *current++)]; } - return remainder; + remainder = static_cast(remainder >> SHIFT); + } + + return remainder; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -780,8 +816,8 @@ inline CRCType CRC::CalculateRemainder(const void * data, crcpp_size size, const */ inline const CRC::Parameters & CRC::CRC_4_ITU() { - static const Parameters parameters = { 0x3, 0x0, 0x0, true, true }; - return parameters; + static const Parameters parameters = {0x3, 0x0, 0x0, true, true}; + return parameters; } /** @@ -798,8 +834,8 @@ inline const CRC::Parameters & CRC::CRC_4_ITU() */ inline const CRC::Parameters & CRC::CRC_5_EPC() { - static const Parameters parameters = { 0x09, 0x09, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x09, 0x09, 0x00, false, false}; + return parameters; } /** @@ -816,8 +852,8 @@ inline const CRC::Parameters & CRC::CRC_5_EPC() */ inline const CRC::Parameters & CRC::CRC_5_ITU() { - static const Parameters parameters = { 0x15, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x15, 0x00, 0x00, true, true}; + return parameters; } /** @@ -834,8 +870,8 @@ inline const CRC::Parameters & CRC::CRC_5_ITU() */ inline const CRC::Parameters & CRC::CRC_5_USB() { - static const Parameters parameters = { 0x05, 0x1F, 0x1F, true, true }; - return parameters; + static const Parameters parameters = {0x05, 0x1F, 0x1F, true, true}; + return parameters; } /** @@ -852,8 +888,8 @@ inline const CRC::Parameters & CRC::CRC_5_USB() */ inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() { - static const Parameters parameters = { 0x27, 0x3F, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x27, 0x3F, 0x00, false, false}; + return parameters; } /** @@ -870,8 +906,8 @@ inline const CRC::Parameters & CRC::CRC_6_CDMA2000A() */ inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() { - static const Parameters parameters = { 0x07, 0x3F, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x07, 0x3F, 0x00, false, false}; + return parameters; } /** @@ -888,8 +924,8 @@ inline const CRC::Parameters & CRC::CRC_6_CDMA2000B() */ inline const CRC::Parameters & CRC::CRC_6_ITU() { - static const Parameters parameters = { 0x03, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x03, 0x00, 0x00, true, true}; + return parameters; } /** @@ -906,8 +942,8 @@ inline const CRC::Parameters & CRC::CRC_6_ITU() */ inline const CRC::Parameters & CRC::CRC_7() { - static const Parameters parameters = { 0x09, 0x00, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x09, 0x00, 0x00, false, false}; + return parameters; } #endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -925,8 +961,8 @@ inline const CRC::Parameters & CRC::CRC_7() */ inline const CRC::Parameters & CRC::CRC_8() { - static const Parameters parameters = { 0x07, 0x00, 0x00, false, false }; - return parameters; + static const Parameters parameters = {0x07, 0x00, 0x00, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -944,8 +980,8 @@ inline const CRC::Parameters & CRC::CRC_8() */ inline const CRC::Parameters & CRC::CRC_8_EBU() { - static const Parameters parameters = { 0x1D, 0xFF, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x1D, 0xFF, 0x00, true, true}; + return parameters; } /** @@ -962,8 +998,8 @@ inline const CRC::Parameters & CRC::CRC_8_EBU() */ inline const CRC::Parameters & CRC::CRC_8_MAXIM() { - static const Parameters parameters = { 0x31, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x31, 0x00, 0x00, true, true}; + return parameters; } /** @@ -980,8 +1016,8 @@ inline const CRC::Parameters & CRC::CRC_8_MAXIM() */ inline const CRC::Parameters & CRC::CRC_8_WCDMA() { - static const Parameters parameters = { 0x9B, 0x00, 0x00, true, true }; - return parameters; + static const Parameters parameters = {0x9B, 0x00, 0x00, true, true}; + return parameters; } /** @@ -998,8 +1034,8 @@ inline const CRC::Parameters & CRC::CRC_8_WCDMA() */ inline const CRC::Parameters & CRC::CRC_10() { - static const Parameters parameters = { 0x233, 0x000, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x233, 0x000, 0x000, false, false}; + return parameters; } /** @@ -1016,8 +1052,8 @@ inline const CRC::Parameters & CRC::CRC_10() */ inline const CRC::Parameters & CRC::CRC_10_CDMA2000() { - static const Parameters parameters = { 0x3D9, 0x3FF, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x3D9, 0x3FF, 0x000, false, false}; + return parameters; } /** @@ -1034,8 +1070,8 @@ inline const CRC::Parameters & CRC::CRC_10_CDMA2000() */ inline const CRC::Parameters & CRC::CRC_11() { - static const Parameters parameters = { 0x385, 0x01A, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x385, 0x01A, 0x000, false, false}; + return parameters; } /** @@ -1052,8 +1088,8 @@ inline const CRC::Parameters & CRC::CRC_11() */ inline const CRC::Parameters & CRC::CRC_12_CDMA2000() { - static const Parameters parameters = { 0xF13, 0xFFF, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0xF13, 0xFFF, 0x000, false, false}; + return parameters; } /** @@ -1070,8 +1106,8 @@ inline const CRC::Parameters & CRC::CRC_12_CDMA2000() */ inline const CRC::Parameters & CRC::CRC_12_DECT() { - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, false }; - return parameters; + static const Parameters parameters = {0x80F, 0x000, 0x000, false, false}; + return parameters; } /** @@ -1088,8 +1124,8 @@ inline const CRC::Parameters & CRC::CRC_12_DECT() */ inline const CRC::Parameters & CRC::CRC_12_UMTS() { - static const Parameters parameters = { 0x80F, 0x000, 0x000, false, true }; - return parameters; + static const Parameters parameters = {0x80F, 0x000, 0x000, false, true}; + return parameters; } /** @@ -1106,8 +1142,8 @@ inline const CRC::Parameters & CRC::CRC_12_UMTS() */ inline const CRC::Parameters & CRC::CRC_13_BBC() { - static const Parameters parameters = { 0x1CF5, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x1CF5, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1124,8 +1160,8 @@ inline const CRC::Parameters & CRC::CRC_13_BBC() */ inline const CRC::Parameters & CRC::CRC_15() { - static const Parameters parameters = { 0x4599, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x4599, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1142,8 +1178,8 @@ inline const CRC::Parameters & CRC::CRC_15() */ inline const CRC::Parameters & CRC::CRC_15_MPT1327() { - static const Parameters parameters = { 0x6815, 0x0000, 0x0001, false, false }; - return parameters; + static const Parameters parameters = {0x6815, 0x0000, 0x0001, false, false}; + return parameters; } #endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1161,8 +1197,8 @@ inline const CRC::Parameters & CRC::CRC_15_MPT1327() */ inline const CRC::Parameters & CRC::CRC_16_ARC() { - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0x0000, 0x0000, true, true}; + return parameters; } /** @@ -1179,8 +1215,8 @@ inline const CRC::Parameters & CRC::CRC_16_ARC() */ inline const CRC::Parameters & CRC::CRC_16_BUYPASS() { - static const Parameters parameters = { 0x8005, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x8005, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1197,8 +1233,8 @@ inline const CRC::Parameters & CRC::CRC_16_BUYPASS() */ inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() { - static const Parameters parameters = { 0x1021, 0xFFFF, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x1021, 0xFFFF, 0x0000, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1216,8 +1252,8 @@ inline const CRC::Parameters & CRC::CRC_16_CCITTFALSE() */ inline const CRC::Parameters & CRC::CRC_16_CDMA2000() { - static const Parameters parameters = { 0xC867, 0xFFFF, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0xC867, 0xFFFF, 0x0000, false, false}; + return parameters; } /** @@ -1234,8 +1270,8 @@ inline const CRC::Parameters & CRC::CRC_16_CDMA2000() */ inline const CRC::Parameters & CRC::CRC_16_CMS() { - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x8005, 0xFFFF, 0x0000, false, false}; + return parameters; } /** @@ -1252,8 +1288,8 @@ inline const CRC::Parameters & CRC::CRC_16_CMS() */ inline const CRC::Parameters & CRC::CRC_16_DECTR() { - static const Parameters parameters = { 0x0589, 0x0000, 0x0001, false, false }; - return parameters; + static const Parameters parameters = {0x0589, 0x0000, 0x0001, false, false}; + return parameters; } /** @@ -1270,8 +1306,8 @@ inline const CRC::Parameters & CRC::CRC_16_DECTR() */ inline const CRC::Parameters & CRC::CRC_16_DECTX() { - static const Parameters parameters = { 0x0589, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x0589, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1288,8 +1324,8 @@ inline const CRC::Parameters & CRC::CRC_16_DECTX() */ inline const CRC::Parameters & CRC::CRC_16_DNP() { - static const Parameters parameters = { 0x3D65, 0x0000, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x3D65, 0x0000, 0xFFFF, true, true}; + return parameters; } #endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1307,8 +1343,8 @@ inline const CRC::Parameters & CRC::CRC_16_DNP() */ inline const CRC::Parameters & CRC::CRC_16_GENIBUS() { - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x1021, 0xFFFF, 0xFFFF, false, false}; + return parameters; } /** @@ -1325,8 +1361,8 @@ inline const CRC::Parameters & CRC::CRC_16_GENIBUS() */ inline const CRC::Parameters & CRC::CRC_16_KERMIT() { - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, true, true }; - return parameters; + static const Parameters parameters = {0x1021, 0x0000, 0x0000, true, true}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1344,8 +1380,8 @@ inline const CRC::Parameters & CRC::CRC_16_KERMIT() */ inline const CRC::Parameters & CRC::CRC_16_MAXIM() { - static const Parameters parameters = { 0x8005, 0x0000, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0x0000, 0xFFFF, true, true}; + return parameters; } /** @@ -1362,8 +1398,8 @@ inline const CRC::Parameters & CRC::CRC_16_MAXIM() */ inline const CRC::Parameters & CRC::CRC_16_MODBUS() { - static const Parameters parameters = { 0x8005, 0xFFFF, 0x0000, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0xFFFF, 0x0000, true, true}; + return parameters; } /** @@ -1380,8 +1416,8 @@ inline const CRC::Parameters & CRC::CRC_16_MODBUS() */ inline const CRC::Parameters & CRC::CRC_16_T10DIF() { - static const Parameters parameters = { 0x8BB7, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x8BB7, 0x0000, 0x0000, false, false}; + return parameters; } /** @@ -1398,8 +1434,8 @@ inline const CRC::Parameters & CRC::CRC_16_T10DIF() */ inline const CRC::Parameters & CRC::CRC_16_USB() { - static const Parameters parameters = { 0x8005, 0xFFFF, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x8005, 0xFFFF, 0xFFFF, true, true}; + return parameters; } #endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1418,8 +1454,8 @@ inline const CRC::Parameters & CRC::CRC_16_USB() */ inline const CRC::Parameters & CRC::CRC_16_X25() { - static const Parameters parameters = { 0x1021, 0xFFFF, 0xFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x1021, 0xFFFF, 0xFFFF, true, true}; + return parameters; } /** @@ -1436,8 +1472,8 @@ inline const CRC::Parameters & CRC::CRC_16_X25() */ inline const CRC::Parameters & CRC::CRC_16_XMODEM() { - static const Parameters parameters = { 0x1021, 0x0000, 0x0000, false, false }; - return parameters; + static const Parameters parameters = {0x1021, 0x0000, 0x0000, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1455,8 +1491,8 @@ inline const CRC::Parameters & CRC::CRC_16_XMODEM() */ inline const CRC::Parameters & CRC::CRC_17_CAN() { - static const Parameters parameters = { 0x1685B, 0x00000, 0x00000, false, false }; - return parameters; + static const Parameters parameters = {0x1685B, 0x00000, 0x00000, false, false}; + return parameters; } /** @@ -1473,8 +1509,9 @@ inline const CRC::Parameters & CRC::CRC_17_CAN() */ inline const CRC::Parameters & CRC::CRC_21_CAN() { - static const Parameters parameters = { 0x102899, 0x000000, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x102899, 0x000000, 0x000000, false, false}; + return parameters; } /** @@ -1491,8 +1528,9 @@ inline const CRC::Parameters & CRC::CRC_21_CAN() */ inline const CRC::Parameters & CRC::CRC_24() { - static const Parameters parameters = { 0x864CFB, 0xB704CE, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x864CFB, 0xB704CE, 0x000000, false, false}; + return parameters; } /** @@ -1509,8 +1547,9 @@ inline const CRC::Parameters & CRC::CRC_24() */ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() { - static const Parameters parameters = { 0x5D6DCB, 0xFEDCBA, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x5D6DCB, 0xFEDCBA, 0x000000, false, false}; + return parameters; } /** @@ -1527,8 +1566,9 @@ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYA() */ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() { - static const Parameters parameters = { 0x5D6DCB, 0xABCDEF, 0x000000, false, false }; - return parameters; + static const Parameters parameters = {0x5D6DCB, 0xABCDEF, 0x000000, false, false}; + return parameters; } /** @@ -1545,8 +1585,9 @@ inline const CRC::Parameters & CRC::CRC_24_FLEXRAYB() */ inline const CRC::Parameters & CRC::CRC_30() { - static const Parameters parameters = { 0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false }; - return parameters; + static const Parameters parameters = {0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false}; + return parameters; } #endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1564,8 +1605,9 @@ inline const CRC::Parameters & CRC::CRC_30() */ inline const CRC::Parameters & CRC::CRC_32() { - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, true, true}; + return parameters; } /** @@ -1582,8 +1624,9 @@ inline const CRC::Parameters & CRC::CRC_32() */ inline const CRC::Parameters & CRC::CRC_32_BZIP2() { - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0xFFFFFFFF, 0xFFFFFFFF, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1601,8 +1644,9 @@ inline const CRC::Parameters & CRC::CRC_32_BZIP2() */ inline const CRC::Parameters & CRC::CRC_32_C() { - static const Parameters parameters = { 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true }; - return parameters; + static const Parameters parameters = {0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true}; + return parameters; } #endif @@ -1620,8 +1664,9 @@ inline const CRC::Parameters & CRC::CRC_32_C() */ inline const CRC::Parameters & CRC::CRC_32_MPEG2() { - static const Parameters parameters = { 0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0xFFFFFFFF, 0x00000000, false, false}; + return parameters; } /** @@ -1638,8 +1683,9 @@ inline const CRC::Parameters & CRC::CRC_32_MPEG2() */ inline const CRC::Parameters & CRC::CRC_32_POSIX() { - static const Parameters parameters = { 0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x04C11DB7, 0x00000000, 0xFFFFFFFF, false, false}; + return parameters; } #ifdef CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS @@ -1657,8 +1703,9 @@ inline const CRC::Parameters & CRC::CRC_32_POSIX() */ inline const CRC::Parameters & CRC::CRC_32_Q() { - static const Parameters parameters = { 0x814141AB, 0x00000000, 0x00000000, false, false }; - return parameters; + static const Parameters parameters = {0x814141AB, 0x00000000, 0x00000000, false, false}; + return parameters; } /** @@ -1675,8 +1722,9 @@ inline const CRC::Parameters & CRC::CRC_32_Q() */ inline const CRC::Parameters & CRC::CRC_40_GSM() { - static const Parameters parameters = { 0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false }; - return parameters; + static const Parameters parameters = {0x0004820009, 0x0000000000, 0xFFFFFFFFFF, false, false}; + return parameters; } /** @@ -1693,8 +1741,9 @@ inline const CRC::Parameters & CRC::CRC_40_GSM() */ inline const CRC::Parameters & CRC::CRC_64() { - static const Parameters parameters = { 0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false }; - return parameters; + static const Parameters parameters = {0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false}; + return parameters; } #endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index e1205df..dade2a9 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -48,7 +48,7 @@ using vesc_msgs::msg::VescState; using vesc_msgs::msg::VescStateStamped; class VescDriver -: public rclcpp::Node + : public rclcpp::Node { public: VescDriver(const rclcpp::NodeOptions & options); @@ -65,8 +65,8 @@ class VescDriver CommandLimit( rclcpp::Node * node_ptr, const std::string & str, - const boost::optional& min_lower = boost::optional(), - const boost::optional& max_upper = boost::optional()); + const boost::optional & min_lower = boost::optional(), + const boost::optional & max_upper = boost::optional()); double clip(double value); rclcpp::Node * node_ptr; rclcpp::Logger logger; diff --git a/vesc_driver/include/vesc_driver/vesc_interface.hpp b/vesc_driver/include/vesc_driver/vesc_interface.hpp index 21b41d9..27bd908 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.hpp +++ b/vesc_driver/include/vesc_driver/vesc_interface.hpp @@ -46,8 +46,8 @@ namespace vesc_driver class VescInterface { public: - typedef std::function PacketHandlerFunction; - typedef std::function ErrorHandlerFunction; + typedef std::function PacketHandlerFunction; + typedef std::function ErrorHandlerFunction; /** * Creates a VescInterface object. Opens the serial port interface to the VESC if @p port is not @@ -60,9 +60,10 @@ class VescInterface * * @throw SerialException */ - VescInterface(const std::string& port = std::string(), - const PacketHandlerFunction& packet_handler = PacketHandlerFunction(), - const ErrorHandlerFunction& error_handler = ErrorHandlerFunction()); + VescInterface( + const std::string & port = std::string(), + const PacketHandlerFunction & packet_handler = PacketHandlerFunction(), + const ErrorHandlerFunction & error_handler = ErrorHandlerFunction()); /** * Delete copy constructor and equals operator. @@ -78,20 +79,20 @@ class VescInterface /** * Sets / updates the function that this class calls when a VESC packet is received. */ - void setPacketHandler(const PacketHandlerFunction& handler); + void setPacketHandler(const PacketHandlerFunction & handler); /** * Sets / updates the function that this class calls when an error is detected, such as a bad * checksum. */ - void setErrorHandler(const ErrorHandlerFunction& handler); + void setErrorHandler(const ErrorHandlerFunction & handler); /** * Opens the serial port interface to the VESC. * * @throw SerialException */ - void connect(const std::string& port); + void connect(const std::string & port); /** * Closes the serial port interface to the VESC. @@ -108,7 +109,7 @@ class VescInterface /** * Send a VESC packet. */ - void send(const VescPacket& packet); + void send(const VescPacket & packet); void requestFWVersion(); void requestState(); @@ -129,18 +130,20 @@ class VescInterface class SerialException : public std::exception { // Disable copy constructors - SerialException& operator=(const SerialException&); + SerialException & operator=(const SerialException &); std::string e_what_; + public: - explicit SerialException(const char *description) + explicit SerialException(const char * description) { std::stringstream ss; ss << "SerialException " << description << " failed."; e_what_ = ss.str(); } - SerialException(const SerialException& other) : e_what_(other.e_what_) {} + SerialException(const SerialException & other) + : e_what_(other.e_what_) {} virtual ~SerialException() throw() {} - virtual const char* what() const throw() + virtual const char * what() const throw() { return e_what_.c_str(); } diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index c671a64..e0f560b 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -51,7 +51,7 @@ class VescFrame virtual ~VescFrame() {} // getters - virtual const Buffer& frame() const + virtual const Buffer & frame() const { return *frame_; } @@ -65,7 +65,8 @@ class VescFrame static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value /** CRC parameters for the VESC */ - static constexpr CRC::Parameters CRC_TYPE = { 0x1021, 0x0000, 0x0000, false, false }; + static constexpr CRC::Parameters CRC_TYPE = {0x1021, 0x0000, 0x0000, false, false}; protected: /** Construct frame with specified payload size. */ @@ -76,7 +77,7 @@ class VescFrame private: /** Construct from buffer. Used by VescPacketFactory factory. */ - VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload); + VescFrame(const BufferRangeConst & frame, const BufferRangeConst & payload); /** Give VescPacketFactory access to private constructor. */ friend class VescPacketFactory; @@ -90,14 +91,14 @@ class VescPacket : public VescFrame public: virtual ~VescPacket() {} - virtual const std::string& name() const + virtual const std::string & name() const { return name_; } protected: - VescPacket(const std::string& name, int payload_size, int payload_id); - VescPacket(const std::string& name, std::shared_ptr raw); + VescPacket(const std::string & name, int payload_size, int payload_id); + VescPacket(const std::string & name, std::shared_ptr raw); private: std::string name_; diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp index 377538a..bcf8a78 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp @@ -47,7 +47,7 @@ class VescPacketFactory { public: /** Return the global factory object */ - static VescPacketFactory* getFactory(); + static VescPacketFactory * getFactory(); /** * Create a VescPacket from a buffer (factory function). Packet must start (start of frame @@ -69,9 +69,10 @@ class VescPacketFactory * * @return Pointer to a valid VescPacket if successful. Otherwise, an empty pointer. */ - static VescPacketPtr createPacket(const Buffer::const_iterator& begin, - const Buffer::const_iterator& end, - int* num_bytes_needed, std::string* what); + static VescPacketPtr createPacket( + const Buffer::const_iterator & begin, + const Buffer::const_iterator & end, + int * num_bytes_needed, std::string * what); typedef std::function)> CreateFn; @@ -86,8 +87,8 @@ class VescPacketFactory private: VescPacketFactory(); - typedef std::map FactoryMap; - static FactoryMap* getMap(); + typedef std::map FactoryMap; + static FactoryMap * getMap(); }; template @@ -107,7 +108,7 @@ class PacketFactoryTemplate /** Use this macro to register packets */ #define REGISTER_PACKET_TYPE(id, klass) \ -static PacketFactoryTemplate global_##klass##Factory((id)); + static PacketFactoryTemplate global_ ## klass ## Factory((id)); } // namespace vesc_driver diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 516c095..40c8362 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -46,7 +46,7 @@ using std_msgs::msg::Float64; using vesc_msgs::msg::VescStateStamped; VescDriver::VescDriver(const rclcpp::NodeOptions & options) - : rclcpp::Node("vesc_driver", options), +: rclcpp::Node("vesc_driver", options), vesc_( std::string(), std::bind(&VescDriver::vescPacketCallback, this, _1), @@ -83,7 +83,9 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) // subscribe to motor and servo command topics duty_cycle_sub_ = create_subscription( - "commands/motor/duty_cycle", rclcpp::QoS{10}, std::bind(&VescDriver::dutyCycleCallback, this, _1)); + "commands/motor/duty_cycle", rclcpp::QoS{10}, std::bind( + &VescDriver::dutyCycleCallback, this, + _1)); current_sub_ = create_subscription( "commands/motor/current", rclcpp::QoS{10}, std::bind(&VescDriver::currentCallback, this, _1)); brake_sub_ = create_subscription( @@ -115,8 +117,7 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) void VescDriver::timerCallback() { // VESC interface should not unexpectedly disconnect, but test for it anyway - if (!vesc_.isConnected()) - { + if (!vesc_.isConnected()) { RCLCPP_FATAL(get_logger(), "Unexpectedly disconnected from serial port."); rclcpp::shutdown(); return; @@ -127,25 +128,19 @@ void VescDriver::timerCallback() * INITIALIZING - request and wait for vesc version * OPERATING - receiving commands from subscriber topics */ - if (driver_mode_ == MODE_INITIALIZING) - { + if (driver_mode_ == MODE_INITIALIZING) { // request version number, return packet will update the internal version numbers vesc_.requestFWVersion(); - if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) - { + if (fw_version_major_ >= 0 && fw_version_minor_ >= 0) { RCLCPP_INFO( get_logger(), "Connected to VESC with firmware version %d.%d", fw_version_major_, fw_version_minor_); driver_mode_ = MODE_OPERATING; } - } - else if (driver_mode_ == MODE_OPERATING) - { + } else if (driver_mode_ == MODE_OPERATING) { // poll for vesc state (telemetry) vesc_.requestState(); - } - else - { + } else { // unknown mode, how did that happen? assert(false && "unknown driver mode"); } @@ -153,8 +148,7 @@ void VescDriver::timerCallback() void VescDriver::vescPacketCallback(const std::shared_ptr & packet) { - if (packet->name() == "Values") - { + if (packet->name() == "Values") { std::shared_ptr values = std::dynamic_pointer_cast(packet); @@ -175,9 +169,7 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa state_msg.state.fault_code = values->fault_code(); state_pub_->publish(state_msg); - } - else if (packet->name() == "FWVersion") - { + } else if (packet->name() == "FWVersion") { std::shared_ptr fw_version = std::dynamic_pointer_cast(packet); // todo: might need lock here @@ -198,8 +190,7 @@ void VescDriver::vescErrorCallback(const std::string & error) */ void VescDriver::dutyCycleCallback(const Float64::SharedPtr duty_cycle) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ = MODE_OPERATING) { vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } } @@ -211,8 +202,7 @@ void VescDriver::dutyCycleCallback(const Float64::SharedPtr duty_cycle) */ void VescDriver::currentCallback(const Float64::SharedPtr current) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ = MODE_OPERATING) { vesc_.setCurrent(current_limit_.clip(current->data)); } } @@ -224,8 +214,7 @@ void VescDriver::currentCallback(const Float64::SharedPtr current) */ void VescDriver::brakeCallback(const Float64::SharedPtr brake) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ = MODE_OPERATING) { vesc_.setBrake(brake_limit_.clip(brake->data)); } } @@ -238,8 +227,7 @@ void VescDriver::brakeCallback(const Float64::SharedPtr brake) */ void VescDriver::speedCallback(const Float64::SharedPtr speed) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ = MODE_OPERATING) { vesc_.setSpeed(speed_limit_.clip(speed->data)); } } @@ -250,8 +238,7 @@ void VescDriver::speedCallback(const Float64::SharedPtr speed) */ void VescDriver::positionCallback(const Float64::SharedPtr position) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ = MODE_OPERATING) { // ROS uses radians but VESC seems to use degrees. Convert to degrees. double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; vesc_.setPosition(position_deg); @@ -263,8 +250,7 @@ void VescDriver::positionCallback(const Float64::SharedPtr position) */ void VescDriver::servoCallback(const Float64::SharedPtr servo) { - if (driver_mode_ = MODE_OPERATING) - { + if (driver_mode_ = MODE_OPERATING) { double servo_clipped(servo_limit_.clip(servo->data)); vesc_.setServo(servo_clipped); // publish clipped servo value as a "sensor" @@ -277,39 +263,31 @@ void VescDriver::servoCallback(const Float64::SharedPtr servo) VescDriver::CommandLimit::CommandLimit( rclcpp::Node * node_ptr, const std::string & str, - const boost::optional& min_lower, - const boost::optional& max_upper) - : node_ptr(node_ptr), - logger(node_ptr->get_logger()), - name(str) + const boost::optional & min_lower, + const boost::optional & max_upper) +: node_ptr(node_ptr), + logger(node_ptr->get_logger()), + name(str) { // check if user's minimum value is outside of the range min_lower to max_upper auto param_min = node_ptr->declare_parameter(name + "_min", rclcpp::ParameterValue(0.0)); - if (param_min.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) - { - if (min_lower && param_min.get() < *min_lower) - { + if (param_min.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + if (min_lower && param_min.get() < *min_lower) { lower = *min_lower; RCLCPP_WARN_STREAM( logger, "Parameter " << name << "_min (" << param_min.get() << - ") is less than the feasible minimum (" << *min_lower << ")."); - } - else if (max_upper && param_min.get() > *max_upper) - { + ") is less than the feasible minimum (" << *min_lower << ")."); + } else if (max_upper && param_min.get() > *max_upper) { lower = *max_upper; RCLCPP_WARN_STREAM( logger, "Parameter " << name << "_min (" << param_min.get() << - ") is greater than the feasible maximum (" << *max_upper << ")."); - } - else - { + ") is greater than the feasible maximum (" << *max_upper << ")."); + } else { lower = param_min.get(); } - } - else if (min_lower) - { + } else if (min_lower) { lower = *min_lower; } @@ -317,38 +295,29 @@ VescDriver::CommandLimit::CommandLimit( auto param_max = node_ptr->declare_parameter(name + "_max", rclcpp::ParameterValue(0.0)); - if (param_max.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) - { - if (min_lower && param_max.get() < *min_lower) - { + if (param_max.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) { + if (min_lower && param_max.get() < *min_lower) { upper = *min_lower; RCLCPP_WARN_STREAM( logger, "Parameter " << name << "_max (" << param_max.get() << - ") is less than the feasible minimum (" << *min_lower << ")."); - } - else if (max_upper && param_max.get() > *max_upper) - { + ") is less than the feasible minimum (" << *min_lower << ")."); + } else if (max_upper && param_max.get() > *max_upper) { upper = *max_upper; RCLCPP_WARN_STREAM( logger, "Parameter " << name << "_max (" << param_max.get() << - ") is greater than the feasible maximum (" << *max_upper << ")."); - } - else - { + ") is greater than the feasible maximum (" << *max_upper << ")."); + } else { upper = param_max.get(); } - } - else if (max_upper) - { + } else if (max_upper) { upper = *max_upper; } // check for min > max - if (upper && lower && *lower > *upper) - { + if (upper && lower && *lower > *upper) { RCLCPP_WARN_STREAM( logger, "Parameter " << name << "_max (" << *upper << - ") is less than parameter " << name << "_min (" << *lower << ")."); + ") is less than parameter " << name << "_min (" << *lower << ")."); double temp(*lower); lower = *upper; upper = temp; @@ -376,15 +345,13 @@ double VescDriver::CommandLimit::clip(double value) { auto clock = rclcpp::Clock(RCL_ROS_TIME); - if (lower && value < lower) - { + if (lower && value < lower) { RCLCPP_INFO_THROTTLE( logger, clock, 10, "%s command value (%f) below minimum limit (%f), clipping.", name.c_str(), value, *lower); return *lower; } - if (upper && value > upper) - { + if (upper && value > upper) { RCLCPP_INFO_THROTTLE( logger, clock, 10, "%s command value (%f) above maximum limit (%f), clipping.", name.c_str(), value, *upper); diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 0aea0fc..7b601ab 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -28,6 +28,8 @@ #include "vesc_driver/vesc_interface.hpp" #include "vesc_driver/vesc_packet_factory.hpp" +#include +#include #include #include @@ -43,56 +45,54 @@ 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() + : io_service_(), + serial_port_(io_service_) {} - void* rxThread(void); + void * rxThread(void); - static void* rxThreadHelper(void *context) + static void * rxThreadHelper(void * context) { - return ((VescInterface::Impl*)context)->rxThread(); + return ((VescInterface::Impl *)context)->rxThread(); } pthread_t rx_thread_; bool rx_thread_run_; PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; - serial::Serial serial_; + boost::asio::serial_port serial_port_; + +private: + boost::asio::io_service io_service_; }; -void* VescInterface::Impl::rxThread(void) +void * VescInterface::Impl::rxThread(void) { Buffer buffer; buffer.reserve(4096); - while (rx_thread_run_) - { + while (rx_thread_run_) { int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer.empty()) - { + if (!buffer.empty()) { // search buffer for valid packet(s) Buffer::iterator iter(buffer.begin()); Buffer::iterator iter_begin(buffer.begin()); - while (iter != buffer.end()) - { + while (iter != buffer.end()) { // check if valid start-of-frame character if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || - VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) + VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) { // good start, now attempt to create packet std::string error; VescPacketConstPtr packet = VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); - if (packet) - { + if (packet) { // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) - { + if (std::distance(iter_begin, iter) > 0) { std::ostringstream ss; - ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " - << std::distance(iter_begin, iter) << " bytes."; + ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << + std::distance(iter_begin, iter) << " bytes."; error_handler_(ss.str()); } // call packet handler @@ -102,14 +102,10 @@ void* VescInterface::Impl::rxThread(void) iter_begin = iter; // continue to look for another frame in buffer continue; - } - else if (bytes_needed > 0) - { + } else if (bytes_needed > 0) { // need more data, break out of while loop break; // for (iter_sof... - } - else - { + } else { // else, this was not a packet, move on to next byte error_handler_(error); } @@ -119,12 +115,12 @@ void* VescInterface::Impl::rxThread(void) } // if iter is at the end of the buffer, more bytes are needed - if (iter == buffer.end()) + if (iter == buffer.end()) { bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + } // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) - { + if (std::distance(iter_begin, iter) > 0) { std::ostringstream ss; ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; error_handler_(ss.str()); @@ -133,27 +129,30 @@ void* VescInterface::Impl::rxThread(void) } // attempt to read at least bytes_needed bytes from the serial port - int bytes_to_read = - std::max(bytes_needed, std::min(4096, static_cast(serial_.available()))); - int bytes_read = serial_.read(buffer, bytes_to_read); - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) - { + int bytes_to_read = std::max(bytes_needed, 4096); + const size_t bytes_read = boost::asio::read( + serial_port_, + boost::asio::buffer(buffer, buffer.size()), + boost::asio::transfer_exactly(bytes_to_read)); + + if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); } } } - -VescInterface::VescInterface(const std::string& port, - const PacketHandlerFunction& packet_handler, - const ErrorHandlerFunction& error_handler) : - impl_(new Impl()) +VescInterface::VescInterface( + const std::string & port, + const PacketHandlerFunction & packet_handler, + const ErrorHandlerFunction & error_handler) +: impl_(new Impl()) { setPacketHandler(packet_handler); setErrorHandler(error_handler); // attempt to conect if the port is specified - if (!port.empty()) + if (!port.empty()) { connect(port); + } } VescInterface::~VescInterface() @@ -161,35 +160,40 @@ VescInterface::~VescInterface() disconnect(); } -void VescInterface::setPacketHandler(const PacketHandlerFunction& handler) +void VescInterface::setPacketHandler(const PacketHandlerFunction & handler) { // todo - definately need mutex impl_->packet_handler_ = handler; } -void VescInterface::setErrorHandler(const ErrorHandlerFunction& handler) +void VescInterface::setErrorHandler(const ErrorHandlerFunction & handler) { // todo - definately need mutex impl_->error_handler_ = handler; } -void VescInterface::connect(const std::string& port) +void VescInterface::connect(const std::string & port) { // todo - mutex? - if (isConnected()) - { + if (isConnected()) { throw SerialException("Already connected to serial port."); } // connect to serial port - try - { - impl_->serial_.setPort(port); - impl_->serial_.open(); - } - catch (const std::exception& e) - { + try { + impl_->serial_port_.open(port); + impl_->serial_port_.set_option(boost::asio::serial_port_base::baud_rate(115200)); + impl_->serial_port_.set_option( + boost::asio::serial_port::flow_control( + boost::asio::serial_port::flow_control::none)); + impl_->serial_port_.set_option( + boost::asio::serial_port::parity( + boost::asio::serial_port::parity::none)); + impl_->serial_port_.set_option( + boost::asio::serial_port::stop_bits( + boost::asio::serial_port::stop_bits::one)); + } catch (const std::exception & e) { std::stringstream ss; ss << "Failed to open the serial port to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); @@ -206,29 +210,34 @@ void VescInterface::disconnect() { // todo - mutex? - if (isConnected()) - { + if (isConnected()) { // bring down read thread impl_->rx_thread_run_ = false; int result = pthread_join(impl_->rx_thread_, NULL); assert(0 == result); - impl_->serial_.close(); + impl_->serial_port_.close(); } } bool VescInterface::isConnected() const { - return impl_->serial_.isOpen(); + return impl_->serial_port_.is_open(); } -void VescInterface::send(const VescPacket& packet) +void VescInterface::send(const VescPacket & packet) { - size_t written = impl_->serial_.write(packet.frame()); - if (written != packet.frame().size()) - { + try { + size_t written = impl_->serial_port_.write_some( + boost::asio::buffer(packet.frame())); + if (written != packet.frame().size()) { + std::stringstream ss; + ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; + throw SerialException(ss.str().c_str()); + } + } catch (const std::exception & e) { std::stringstream ss; - ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; + ss << "Failed to open the serial port to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } } diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index f7555b1..090e5cd 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -43,16 +43,13 @@ VescFrame::VescFrame(int payload_size) { assert(payload_size >= 0 && payload_size <= 1024); - if (payload_size < 256) - { + if (payload_size < 256) { // single byte payload size frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + payload_size)); *frame_->begin() = 2; *(frame_->begin() + 1) = payload_size; payload_.first = frame_->begin() + 2; - } - else - { + } else { // two byte payload size frame_.reset(new Buffer(VESC_MIN_FRAME_SIZE + 1 + payload_size)); *frame_->begin() = 3; @@ -65,38 +62,39 @@ VescFrame::VescFrame(int payload_size) *(frame_->end() - 1) = 3; } -VescFrame::VescFrame(const BufferRangeConst& frame, const BufferRangeConst& payload) +VescFrame::VescFrame(const BufferRangeConst & frame, const BufferRangeConst & payload) { /* VescPacketFactory::createPacket() should make sure that the input is valid, but run a few cheap checks anyway */ assert(std::distance(frame.first, frame.second) >= VESC_MIN_FRAME_SIZE); assert(std::distance(frame.first, frame.second) <= VESC_MAX_FRAME_SIZE); assert(std::distance(payload.first, payload.second) <= VESC_MAX_PAYLOAD_SIZE); - assert(std::distance(frame.first, payload.first) > 0 && - std::distance(payload.second, frame.second) > 0); + assert( + std::distance(frame.first, payload.first) > 0 && + std::distance(payload.second, frame.second) > 0); frame_.reset(new Buffer(frame.first, frame.second)); payload_.first = frame_->begin() + std::distance(frame.first, payload.first); payload_.second = frame_->begin() + std::distance(frame.first, payload.second); } -VescPacket::VescPacket(const std::string& name, int payload_size, int payload_id) : - VescFrame(payload_size), name_(name) +VescPacket::VescPacket(const std::string & name, int payload_size, int payload_id) +: VescFrame(payload_size), name_(name) { assert(payload_id >= 0 && payload_id < 256); assert(std::distance(payload_.first, payload_.second) > 0); *payload_.first = payload_id; } -VescPacket::VescPacket(const std::string& name, std::shared_ptr raw) : - VescFrame(*raw), name_(name) +VescPacket::VescPacket(const std::string & name, std::shared_ptr raw) +: VescFrame(*raw), name_(name) { } /*------------------------------------------------------------------------------------------------*/ -VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : - VescPacket("FWVersion", raw) +VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) +: VescPacket("FWVersion", raw) { } @@ -112,8 +110,8 @@ 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); @@ -123,40 +121,40 @@ VescPacketRequestFWVersion::VescPacketRequestFWVersion() : /*------------------------------------------------------------------------------------------------*/ -VescPacketValues::VescPacketValues(std::shared_ptr raw) : - VescPacket("Values", raw) +VescPacketValues::VescPacketValues(std::shared_ptr raw) +: VescPacket("Values", raw) { } double VescPacketValues::temp_mos1() const { int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); + static_cast(*(payload_.first + 2))); 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))); + static_cast(*(payload_.first + 4))); 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))); + (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))); + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + static_cast(*(payload_.first + 12))); return static_cast(v) / 100.0; } @@ -164,52 +162,52 @@ double VescPacketValues::current_in() const double VescPacketValues::duty_now() const { int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); + static_cast(*(payload_.first + 22))); return static_cast(v) / 1000.0; } double VescPacketValues::rpm() const { int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26))); + (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))); + (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))); + (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))); + (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))); + (static_cast(*(payload_.first + 40)) << 16) + + (static_cast(*(payload_.first + 41)) << 8) + + static_cast(*(payload_.first + 42))); return static_cast(v); } @@ -244,8 +242,8 @@ 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); @@ -256,8 +254,8 @@ VescPacketRequestValues::VescPacketRequestValues() : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetDuty::VescPacketSetDuty(double duty) : - VescPacket("SetDuty", 5, COMM_SET_DUTY) +VescPacketSetDuty::VescPacketSetDuty(double duty) +: VescPacket("SetDuty", 5, COMM_SET_DUTY) { /** @todo range check duty */ @@ -276,8 +274,8 @@ VescPacketSetDuty::VescPacketSetDuty(double duty) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetCurrent::VescPacketSetCurrent(double current) : - VescPacket("SetCurrent", 5, COMM_SET_CURRENT) +VescPacketSetCurrent::VescPacketSetCurrent(double current) +: VescPacket("SetCurrent", 5, COMM_SET_CURRENT) { int32_t v = static_cast(current * 1000.0); @@ -294,8 +292,8 @@ VescPacketSetCurrent::VescPacketSetCurrent(double current) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : - VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) +VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) +: VescPacket("SetCurrentBrake", 5, COMM_SET_CURRENT_BRAKE) { int32_t v = static_cast(current_brake * 1000.0); @@ -312,8 +310,8 @@ VescPacketSetCurrentBrake::VescPacketSetCurrentBrake(double current_brake) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetRPM::VescPacketSetRPM(double rpm) : - VescPacket("SetRPM", 5, COMM_SET_RPM) +VescPacketSetRPM::VescPacketSetRPM(double rpm) +: VescPacket("SetRPM", 5, COMM_SET_RPM) { int32_t v = static_cast(rpm); @@ -330,8 +328,8 @@ VescPacketSetRPM::VescPacketSetRPM(double rpm) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetPos::VescPacketSetPos(double pos) : - VescPacket("SetPos", 5, COMM_SET_POS) +VescPacketSetPos::VescPacketSetPos(double pos) +: VescPacket("SetPos", 5, COMM_SET_POS) { /** @todo range check pos */ @@ -350,8 +348,8 @@ VescPacketSetPos::VescPacketSetPos(double pos) : /*------------------------------------------------------------------------------------------------*/ -VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) : - VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) +VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) +: VescPacket("SetServoPos", 3, COMM_SET_SERVO_POS) { /** @todo range check pos */ diff --git a/vesc_driver/src/vesc_packet_factory.cpp b/vesc_driver/src/vesc_packet_factory.cpp index 4fbbf36..18d99e4 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -37,7 +37,7 @@ namespace vesc_driver { /** Construct map on first use */ -VescPacketFactory::FactoryMap* VescPacketFactory::getMap() +VescPacketFactory::FactoryMap * VescPacketFactory::getMap() { static FactoryMap m; return &m; @@ -45,49 +45,52 @@ VescPacketFactory::FactoryMap* VescPacketFactory::getMap() void VescPacketFactory::registerPacketType(int payload_id, CreateFn fn) { - FactoryMap* p_map(getMap()); + FactoryMap * p_map(getMap()); assert(0 == p_map->count(payload_id)); (*p_map)[payload_id] = fn; } /** Helper function for when createPacket can not create a packet */ -VescPacketPtr createFailed(int* p_num_bytes_needed, std::string* p_what, - const std::string& what, int num_bytes_needed = 0) +VescPacketPtr createFailed( + int * p_num_bytes_needed, std::string * p_what, + const std::string & what, int num_bytes_needed = 0) { - if (p_num_bytes_needed != NULL) *p_num_bytes_needed = num_bytes_needed; - if (p_what != NULL) *p_what = what; + if (p_num_bytes_needed != NULL) {*p_num_bytes_needed = num_bytes_needed;} + if (p_what != NULL) {*p_what = what;} return VescPacketPtr(); } -VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begin, - const Buffer::const_iterator& end, - int* num_bytes_needed, std::string* what) +VescPacketPtr VescPacketFactory::createPacket( + const Buffer::const_iterator & begin, + const Buffer::const_iterator & end, + int * num_bytes_needed, std::string * what) { // initialize output variables - if (num_bytes_needed != NULL) *num_bytes_needed = 0; - if (what != NULL) what->clear(); + if (num_bytes_needed != NULL) {*num_bytes_needed = 0;} + if (what != NULL) {what->clear();} // need at least VESC_MIN_FRAME_SIZE bytes in buffer int buffer_size(std::distance(begin, end)); - if (buffer_size < VescFrame::VESC_MIN_FRAME_SIZE) - return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", - VescFrame::VESC_MIN_FRAME_SIZE - buffer_size); + if (buffer_size < VescFrame::VESC_MIN_FRAME_SIZE) { + return createFailed( + num_bytes_needed, what, "Buffer does not contain a complete frame", + VescFrame::VESC_MIN_FRAME_SIZE - buffer_size); + } // buffer must begin with a start-of-frame if (VescFrame::VESC_SOF_VAL_SMALL_FRAME != *begin && - VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) + VescFrame::VESC_SOF_VAL_LARGE_FRAME != *begin) + { return createFailed(num_bytes_needed, what, "Buffer must begin with start-of-frame character"); + } // get a view of the payload BufferRangeConst view_payload; - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) - { + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *begin) { // payload size field is one byte view_payload.first = begin + 2; view_payload.second = view_payload.first + *(begin + 1); - } - else - { + } else { assert(VescFrame::VESC_SOF_VAL_LARGE_FRAME == *begin); // payload size field is two bytes view_payload.first = begin + 3; @@ -95,8 +98,9 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi } // check length - if (std::distance(view_payload.first, view_payload.second) > VescFrame::VESC_MAX_PAYLOAD_SIZE) + if (std::distance(view_payload.first, view_payload.second) > VescFrame::VESC_MAX_PAYLOAD_SIZE) { return createFailed(num_bytes_needed, what, "Invalid payload length"); + } // get iterators to crc field, end-of-frame field, and a view of the whole frame Buffer::const_iterator iter_crc(view_payload.second); @@ -105,41 +109,41 @@ VescPacketPtr VescPacketFactory::createPacket(const Buffer::const_iterator& begi // do we have enough data in the buffer to complete the frame? int frame_size = std::distance(view_frame.first, view_frame.second); - if (buffer_size < frame_size) - return createFailed(num_bytes_needed, what, "Buffer does not contain a complete frame", - frame_size - buffer_size); + if (buffer_size < frame_size) { + return createFailed( + num_bytes_needed, what, "Buffer does not contain a complete frame", + frame_size - buffer_size); + } // is the end-of-frame character valid? - if (VescFrame::VESC_EOF_VAL != *iter_eof) + if (VescFrame::VESC_EOF_VAL != *iter_eof) { return createFailed(num_bytes_needed, what, "Invalid end-of-frame character"); + } // is the crc valid? uint16_t crc = (static_cast(*iter_crc) << 8) + *(iter_crc + 1); if (crc != CRC::Calculate( - &(*view_payload.first), std::distance(view_payload.first, view_payload.second), VescFrame::CRC_TYPE)) + &(*view_payload.first), std::distance(view_payload.first, view_payload.second), + VescFrame::CRC_TYPE)) + { return createFailed(num_bytes_needed, what, "Invalid checksum"); + } // frame looks good, construct the raw frame std::shared_ptr raw_frame(new VescFrame(view_frame, view_payload)); // if the packet has a payload, construct the corresponding subclass - if (std::distance(view_payload.first, view_payload.second) > 0) - { + if (std::distance(view_payload.first, view_payload.second) > 0) { // get constructor function from payload id - FactoryMap* p_map(getMap()); + FactoryMap * p_map(getMap()); FactoryMap::const_iterator search(p_map->find(*view_payload.first)); - if (search != p_map->end()) - { + if (search != p_map->end()) { return search->second(raw_frame); - } - else - { + } else { // no subclass constructor for this packet return createFailed(num_bytes_needed, what, "Unkown payload type."); } - } - else - { + } else { // no payload return createFailed(num_bytes_needed, what, "Frame does not have a payload"); } From 840351e28eb77c60c643a2cdcfe679579df8971c Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 16:19:51 -0600 Subject: [PATCH 09/72] [vesc_driver] Fixing copyrights and finishing transition to boost::asio. --- vesc_driver/CMakeLists.txt | 4 +- vesc_driver/include/vesc_driver/crc.hpp | 140 ++++++++++-------- vesc_driver/include/vesc_driver/datatypes.hpp | 77 +++++----- .../include/vesc_driver/vesc_driver.hpp | 53 +++---- .../include/vesc_driver/vesc_interface.hpp | 39 ++--- .../include/vesc_driver/vesc_packet.hpp | 42 +++--- .../vesc_driver/vesc_packet_factory.hpp | 39 ++--- vesc_driver/package.xml | 1 - vesc_driver/src/vesc_driver.cpp | 45 +++--- vesc_driver/src/vesc_interface.cpp | 64 ++++---- vesc_driver/src/vesc_packet.cpp | 44 +++--- vesc_driver/src/vesc_packet_factory.cpp | 39 ++--- 12 files changed, 311 insertions(+), 276 deletions(-) diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index a2adf41..abc9200 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -2,10 +2,10 @@ cmake_minimum_required(VERSION 3.5) project(vesc_driver) # Set minimum C++ standard to C++14 -if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") +if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") message(STATUS "Changing CXX_STANDARD from C++98 to C++14") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") -elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") +elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") message(STATUS "Changing CXX_STANDARD from C++98 to C++14") set(CMAKE_CXX_STANDARD 14) endif() diff --git a/vesc_driver/include/vesc_driver/crc.hpp b/vesc_driver/include/vesc_driver/crc.hpp index 336f7e2..1d85830 100644 --- a/vesc_driver/include/vesc_driver/crc.hpp +++ b/vesc_driver/include/vesc_driver/crc.hpp @@ -1,38 +1,36 @@ -/** - @file CRC.h +// Copyright (c) 2020, Daniel Bahr +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +/** + @file CRC.hpp @author Daniel Bahr @version 1.0.1.0 @copyright - @parblock - CRC++ - Copyright (c) 2020, Daniel Bahr - All rights reserved. - - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the name of CRC++ nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - @endparblock */ /* @@ -55,19 +53,19 @@ #define CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS - Define to include definitions for little-used CRCs. */ -#ifndef VESC_DRIVER_CRC_H_ -#define VESC_DRIVER_CRC_H_ +#ifndef VESC_DRIVER__CRC_HPP_ +#define VESC_DRIVER__CRC_HPP_ -#include // Includes CHAR_BIT +#include // Includes CHAR_BIT #ifdef CRCPP_USE_CPP11 -#include // Includes ::std::size_t -#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t +#include // Includes ::std::size_t +#include // Includes ::std::uint8_t, ::std::uint16_t, ::std::uint32_t, ::std::uint64_t #else -#include // Includes size_t -#include // Includes uint8_t, uint16_t, uint32_t, uint64_t +#include // Includes size_t +#include // Includes uint8_t, uint16_t, uint32_t, uint64_t #endif -#include // Includes ::std::numeric_limits -#include // Includes ::std::move +#include // Includes ::std::numeric_limits +#include // Includes ::std::move #ifndef crcpp_uint8 # ifdef CRCPP_USE_CPP11 @@ -156,7 +154,7 @@ class CRC 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) + bool reflectOutput; ///< true to reflect the output CRC (reflection occurs before the final XOR) // NOLINT Table MakeTable() const; }; @@ -169,10 +167,10 @@ class CRC struct Table { // Constructors are intentionally NOT marked explicit. - Table(const Parameters & parameters); + explicit Table(const Parameters & parameters); #ifdef CRCPP_USE_CPP11 - Table(Parameters && parameters); + explicit Table(Parameters && parameters); #endif const Parameters & GetParameters() const; @@ -211,7 +209,8 @@ class CRC CRCWidth> & 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) + // 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 & CRC_4_ITU(); static const Parameters & CRC_5_EPC(); @@ -402,7 +401,8 @@ inline CRCType CRC::Table::operator[](unsigned char index) co 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) + // 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))); @@ -419,7 +419,8 @@ inline void CRC::Table::InitTable() // 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. + // 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) { @@ -573,7 +574,8 @@ inline IntegerType CRC::Reflect(IntegerType value, crcpp_uint16 numBits) 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) + // 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)); @@ -587,7 +589,8 @@ inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOu /** @brief Undoes the process of computing the final reflection and XOR of a CRC remainder. @note This function allows for computation of multi-part CRCs - @note Calling UndoFinalize() followed by Finalize() (or vice versa) will always return the original remainder value: + @note Calling UndoFinalize() followed by Finalize() (or vice versa) + will always return the original remainder value: CRCType x = ...; CRCType y = Finalize(x, finalXOR, reflectOutput); @@ -604,7 +607,8 @@ inline CRCType CRC::Finalize(CRCType remainder, CRCType finalXOR, bool reflectOu 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) + // 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)); @@ -639,15 +643,17 @@ inline CRCType CRC::CalculateRemainder( ::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. + // Catching this compile-time error is very important. Sadly, + // the compiler error will be very cryptic, but it's better than nothing. + // cppcheck-suppress zerodiv enum { static_assert_failed_CRCType_is_too_small_to_contain_a_CRC_of_width_CRCWidth = 1 / (::std::numeric_limits::digits >= CRCWidth ? 1 : 0) }; #endif const unsigned char * current = reinterpret_cast(data); - // Slightly different implementations based on the parameters. The current implementations try to eliminate as much + // 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); @@ -657,7 +663,8 @@ inline CRCType CRC::CalculateRemainder( // 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: + // 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 @@ -685,7 +692,8 @@ inline CRCType CRC::CalculateRemainder( // 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: + // 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 @@ -718,7 +726,8 @@ inline CRCType CRC::CalculateRemainder( // 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: + // 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 @@ -745,7 +754,8 @@ inline CRCType CRC::CalculateRemainder( @param[in] data Data over which the remainder will be computed @param[in] size Size of the data @param[in] lookupTable CRC lookup table - @param[in] remainder Running CRC remainder. Can be an initial value or the result of a previous CRC remainder calculation. + @param[in] remainder Running CRC remainder. + Can be an initial value or the result of a previous CRC remainder calculation. @tparam CRCType Integer type for storing the CRC result @tparam CRCWidth Number of bits in the CRC @return CRC remainder @@ -945,7 +955,7 @@ inline const CRC::Parameters & CRC::CRC_7() static const Parameters parameters = {0x09, 0x00, 0x00, false, false}; return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-8 SMBus. @@ -1181,7 +1191,7 @@ 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 +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-16 ARC (aka CRC-16 IBM, CRC-16 LHA). @@ -1327,7 +1337,7 @@ 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 +#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). @@ -1438,7 +1448,7 @@ inline const CRC::Parameters & CRC::CRC_16_USB() return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-16 X-25 (aka CRC-16 IBM-SDLC, CRC-16 ISO-HDLC, CRC-16 B). @@ -1589,7 +1599,7 @@ inline const CRC::Parameters & CRC::CRC_30() 30> parameters = {0x2030B9C7, 0x3FFFFFFF, 0x00000000, false, false}; return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS /** @brief Returns a set of parameters for CRC-32 (aka CRC-32 ADCCP, CRC-32 PKZip). @@ -1745,10 +1755,10 @@ inline const CRC::Parameters & CRC::CRC_64() 64> parameters = {0x42F0E1EBA9EA3693, 0x0000000000000000, 0x0000000000000000, false, false}; return parameters; } -#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS +#endif // CRCPP_INCLUDE_ESOTERIC_CRC_DEFINITIONS #ifdef CRCPP_USE_NAMESPACE -} +} // namespace CRCPP #endif -#endif // CRCPP_CRC_H_ +#endif // VESC_DRIVER__CRC_HPP_ diff --git a/vesc_driver/include/vesc_driver/datatypes.hpp b/vesc_driver/include/vesc_driver/datatypes.hpp index 1497a20..e195182 100644 --- a/vesc_driver/include/vesc_driver/datatypes.hpp +++ b/vesc_driver/include/vesc_driver/datatypes.hpp @@ -1,52 +1,57 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. -/* - Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se - This file is part of VESC Tool. +/* + Some parts of this code, Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se - VESC Tool is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. + VESC Tool is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. - VESC Tool is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + VESC Tool is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ + 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 VESC_DRIVER__DATATYPES_HPP_ +#define VESC_DRIVER__DATATYPES_HPP_ #include +namespace vesc_driver +{ + typedef struct { bool isVesc; @@ -209,4 +214,6 @@ typedef enum } NRF_PAIR_RES; -#endif // VESC_DRIVER_DATATYPES_H +} // namespace vesc_driver + +#endif // VESC_DRIVER__DATATYPES_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index dade2a9..68f76ce 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -1,45 +1,48 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- #ifndef VESC_DRIVER__VESC_DRIVER_HPP_ #define VESC_DRIVER__VESC_DRIVER_HPP_ -#include "vesc_driver/vesc_interface.hpp" -#include "vesc_driver/vesc_packet.hpp" - -#include -#include - #include #include #include #include #include +#include +#include + +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet.hpp" + namespace vesc_driver { @@ -51,7 +54,7 @@ class VescDriver : public rclcpp::Node { public: - VescDriver(const rclcpp::NodeOptions & options); + explicit VescDriver(const rclcpp::NodeOptions & options); private: // interface to the VESC diff --git a/vesc_driver/include/vesc_driver/vesc_interface.hpp b/vesc_driver/include/vesc_driver/vesc_interface.hpp index 27bd908..63724a1 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.hpp +++ b/vesc_driver/include/vesc_driver/vesc_interface.hpp @@ -1,27 +1,30 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index e0f560b..a1e4dbd 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -1,27 +1,30 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- @@ -59,7 +62,8 @@ class VescFrame // VESC packet properties static const int VESC_MAX_PAYLOAD_SIZE = 1024; ///< Maximum VESC payload size, in bytes static const int VESC_MIN_FRAME_SIZE = 5; ///< Smallest VESC frame size, in bytes - static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; ///< Largest VESC frame size, in bytes + // v Largest VESC frame size, in bytes + static const int VESC_MAX_FRAME_SIZE = 6 + VESC_MAX_PAYLOAD_SIZE; static const unsigned int VESC_SOF_VAL_SMALL_FRAME = 2; ///< VESC start of "small" frame value static const unsigned int VESC_SOF_VAL_LARGE_FRAME = 3; ///< VESC start of "large" frame value static const unsigned int VESC_EOF_VAL = 3; ///< VESC end-of-frame value diff --git a/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp index bcf8a78..0322d81 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet_factory.hpp @@ -1,27 +1,30 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index c956327..93295f6 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -17,7 +17,6 @@ ament_cmake_auto libboost-dev - libboost-system-dev rclcpp rclcpp_components std_msgs diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 40c8362..6e41459 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -1,32 +1,38 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_driver.hpp" +#include +#include + #include #include #include @@ -34,9 +40,6 @@ #include #include -#include -#include - namespace vesc_driver { diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 7b601ab..22ae196 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -1,43 +1,46 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_interface.hpp" #include "vesc_driver/vesc_packet_factory.hpp" -#include #include -#include #include #include #include #include +#include #include #include +#include namespace vesc_driver { @@ -50,14 +53,9 @@ class VescInterface::Impl serial_port_(io_service_) {} - void * rxThread(void); + void rxThread(); - static void * rxThreadHelper(void * context) - { - return ((VescInterface::Impl *)context)->rxThread(); - } - - pthread_t rx_thread_; + std::unique_ptr rx_thread_; bool rx_thread_run_; PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; @@ -67,7 +65,7 @@ class VescInterface::Impl boost::asio::io_service io_service_; }; -void * VescInterface::Impl::rxThread(void) +void VescInterface::Impl::rxThread() { Buffer buffer; buffer.reserve(4096); @@ -201,9 +199,9 @@ void VescInterface::connect(const std::string & port) // start up a monitoring thread impl_->rx_thread_run_ = true; - int result = - pthread_create(&impl_->rx_thread_, NULL, &VescInterface::Impl::rxThreadHelper, impl_.get()); - assert(0 == result); + impl_->rx_thread_ = std::unique_ptr( + new std::thread( + &VescInterface::Impl::rxThread, impl_.get())); } void VescInterface::disconnect() @@ -213,9 +211,7 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread impl_->rx_thread_run_ = false; - int result = pthread_join(impl_->rx_thread_, NULL); - assert(0 == result); - + impl_->rx_thread_->join(); impl_->serial_port_.close(); } } diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 090e5cd..5bb8df7 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -1,39 +1,43 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_driver/datatypes.hpp" #include "vesc_driver/vesc_packet.hpp" -#include "vesc_driver/vesc_packet_factory.hpp" #include #include #include #include +#include "vesc_driver/datatypes.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" + namespace vesc_driver { diff --git a/vesc_driver/src/vesc_packet_factory.cpp b/vesc_driver/src/vesc_packet_factory.cpp index 18d99e4..de53bb6 100644 --- a/vesc_driver/src/vesc_packet_factory.cpp +++ b/vesc_driver/src/vesc_packet_factory.cpp @@ -1,27 +1,30 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- From 553339337f9be3f792749e1a98eba68a0ff0de03 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 17:30:20 -0600 Subject: [PATCH 10/72] [vesc_driver] Node finally starts. --- vesc_driver/CMakeLists.txt | 16 +++++++-- vesc_driver/launch/vesc_driver_node.launch | 20 ----------- .../launch/vesc_driver_node.launch.xml | 16 +++++++++ vesc_driver/launch/vesc_driver_nodelet.launch | 34 ------------------- vesc_driver/package.xml | 1 + vesc_driver/param/default.params.yaml | 7 ---- vesc_driver/src/vesc_driver.cpp | 5 ++- vesc_driver/src/vesc_interface.cpp | 6 ++-- 8 files changed, 36 insertions(+), 69 deletions(-) delete mode 100644 vesc_driver/launch/vesc_driver_node.launch create mode 100644 vesc_driver/launch/vesc_driver_node.launch.xml delete mode 100644 vesc_driver/launch/vesc_driver_nodelet.launch delete mode 100644 vesc_driver/param/default.params.yaml diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index abc9200..2d1ca80 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -13,17 +13,28 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +find_package(Boost COMPONENTS system REQUIRED) +find_package(Threads) + ########### ## Build ## ########### -# node executable -ament_auto_add_library(${PROJECT_NAME}_node SHARED +# node library +ament_auto_add_library(${PROJECT_NAME} SHARED src/vesc_driver.cpp src/vesc_interface.cpp src/vesc_packet.cpp src/vesc_packet_factory.cpp ) +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} +) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN vesc_driver::VescDriver + EXECUTABLE ${PROJECT_NAME}_node +) ############# ## Testing ## @@ -37,5 +48,4 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch - param ) diff --git a/vesc_driver/launch/vesc_driver_node.launch b/vesc_driver/launch/vesc_driver_node.launch deleted file mode 100644 index f7d4765..0000000 --- a/vesc_driver/launch/vesc_driver_node.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/vesc_driver/launch/vesc_driver_node.launch.xml b/vesc_driver/launch/vesc_driver_node.launch.xml new file mode 100644 index 0000000..b5a730e --- /dev/null +++ b/vesc_driver/launch/vesc_driver_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/vesc_driver/launch/vesc_driver_nodelet.launch b/vesc_driver/launch/vesc_driver_nodelet.launch deleted file mode 100644 index c9f4508..0000000 --- a/vesc_driver/launch/vesc_driver_nodelet.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 93295f6..c956327 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto libboost-dev + libboost-system-dev rclcpp rclcpp_components std_msgs diff --git a/vesc_driver/param/default.params.yaml b/vesc_driver/param/default.params.yaml deleted file mode 100644 index ed5bfb2..0000000 --- a/vesc_driver/param/default.params.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - device_name: "/dev/ttyACM0" - baud_rate: 115200 - flow_control: "none" - parity: "none" - stop_bits: "1" diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 6e41459..07d5760 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -363,5 +363,8 @@ double VescDriver::CommandLimit::clip(double value) return value; } - } // namespace vesc_driver + +#include "rclcpp_components/register_node_macro.hpp" // NOLINT + +RCLCPP_COMPONENTS_REGISTER_NODE(vesc_driver::VescDriver) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 22ae196..12c6981 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -59,10 +59,8 @@ class VescInterface::Impl bool rx_thread_run_; PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; - boost::asio::serial_port serial_port_; - -private: boost::asio::io_service io_service_; + boost::asio::serial_port serial_port_; }; void VescInterface::Impl::rxThread() @@ -211,7 +209,7 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread impl_->rx_thread_run_ = false; - impl_->rx_thread_->join(); + // impl_->rx_thread_->join(); impl_->serial_port_.close(); } } From 1e5c87165d456096c714f5b11d5eb4d349d5c091 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 20:42:55 -0600 Subject: [PATCH 11/72] [vesc_ackermann] Starting port. --- vesc_ackermann/CMakeLists.txt | 28 +++++----- ...ermann_to_vesc.h => ackermann_to_vesc.hpp} | 0 .../{vesc_to_odom.h => vesc_to_odom.hpp} | 0 vesc_ackermann/src/ackermann_to_vesc_node.cpp | 41 -------------- .../src/ackermann_to_vesc_nodelet.cpp | 56 ------------------- vesc_ackermann/src/vesc_to_odom_node.cpp | 41 -------------- vesc_ackermann/src/vesc_to_odom_nodelet.cpp | 56 ------------------- vesc_ackermann/vesc_ackermann_nodelet.xml | 8 --- 8 files changed, 13 insertions(+), 217 deletions(-) rename vesc_ackermann/include/vesc_ackermann/{ackermann_to_vesc.h => ackermann_to_vesc.hpp} (100%) rename vesc_ackermann/include/vesc_ackermann/{vesc_to_odom.h => vesc_to_odom.hpp} (100%) delete mode 100644 vesc_ackermann/src/ackermann_to_vesc_node.cpp delete mode 100644 vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp delete mode 100644 vesc_ackermann/src/vesc_to_odom_node.cpp delete mode 100644 vesc_ackermann/src/vesc_to_odom_nodelet.cpp delete mode 100644 vesc_ackermann/vesc_ackermann_nodelet.xml diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt index 2568d46..3381102 100644 --- a/vesc_ackermann/CMakeLists.txt +++ b/vesc_ackermann/CMakeLists.txt @@ -2,10 +2,10 @@ cmake_minimum_required(VERSION 3.5) project(vesc_ackermann) # Setting C++ standard to 14 -if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") +if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") message(STATUS "Changing CXX_STANDARD from C++98 to C++14") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") -elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") +elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") message(STATUS "Changing CXX_STANDARD from C++98 to C++14") set(CMAKE_CXX_STANDARD 14) endif() @@ -17,25 +17,23 @@ ament_auto_find_build_dependencies() ## Build ## ########### -# node executable -ament_auto_add_executable(vesc_to_odom_node - src/vesc_to_odom_node.cpp +# nodes library +ament_auto_add_library(${PROJECT_NAME} SHARED src/vesc_to_odom.cpp -) - -ament_auto_add_executable(ackermann_to_vesc_node - src/ackermann_to_vesc_node.cpp src/ackermann_to_vesc.cpp ) -# components library -ament_auto_add_library(vesc_ackermann_nodelet - src/ackermann_to_vesc_nodelet.cpp - src/ackermann_to_vesc.cpp - src/vesc_to_odom_nodelet.cpp - src/vesc_to_odom.cpp +# register nodes as components +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN vesc_ackermann::AckermannToVesc + EXECUTABLE ackermann_to_vesc_node +) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN vesc_ackermann::VescToOdom + EXECUTABLE vesc_to_odom_node ) + ############# ## Testing ## ############# diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp similarity index 100% rename from vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.h rename to vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp similarity index 100% rename from vesc_ackermann/include/vesc_ackermann/vesc_to_odom.h rename to vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp diff --git a/vesc_ackermann/src/ackermann_to_vesc_node.cpp b/vesc_ackermann/src/ackermann_to_vesc_node.cpp deleted file mode 100644 index d7fcec0..0000000 --- a/vesc_ackermann/src/ackermann_to_vesc_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include - -#include "vesc_ackermann/ackermann_to_vesc.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "ackermann_to_vesc_node"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - vesc_ackermann::AckermannToVesc ackermann_to_vesc(nh, private_nh); - - ros::spin(); - - return 0; -} diff --git a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp b/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp deleted file mode 100644 index 5b72582..0000000 --- a/vesc_ackermann/src/ackermann_to_vesc_nodelet.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -#include - -#include "vesc_ackermann/ackermann_to_vesc.h" - -namespace vesc_ackermann -{ - -class AckermannToVescNodelet: public nodelet::Nodelet -{ -public: - AckermannToVescNodelet() {} - -private: - virtual void onInit(void); - - std::shared_ptr ackermann_to_vesc_; -}; // class AckermannToVescNodelet - -void AckermannToVescNodelet::onInit() -{ - NODELET_DEBUG("Initializing ackermann to VESC nodelet"); - ackermann_to_vesc_.reset(new AckermannToVesc(getNodeHandle(), getPrivateNodeHandle())); -} - -} // namespace vesc_ackermann - -PLUGINLIB_EXPORT_CLASS(vesc_ackermann::AckermannToVescNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/src/vesc_to_odom_node.cpp b/vesc_ackermann/src/vesc_to_odom_node.cpp deleted file mode 100644 index caea389..0000000 --- a/vesc_ackermann/src/vesc_to_odom_node.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include - -#include "vesc_ackermann/vesc_to_odom.h" - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "vesc_to_odom_node"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); - - vesc_ackermann::VescToOdom vesc_to_odom(nh, private_nh); - - ros::spin(); - - return 0; -} diff --git a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp b/vesc_ackermann/src/vesc_to_odom_nodelet.cpp deleted file mode 100644 index 5f21c85..0000000 --- a/vesc_ackermann/src/vesc_to_odom_nodelet.cpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. -// -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -#include - -#include "vesc_ackermann/vesc_to_odom.h" - -namespace vesc_ackermann -{ - -class VescToOdomNodelet: public nodelet::Nodelet -{ -public: - VescToOdomNodelet() {} - -private: - virtual void onInit(void); - - std::shared_ptr vesc_to_odom_; -}; // class VescToOdomNodelet - -void VescToOdomNodelet::onInit() -{ - NODELET_DEBUG("Initializing RACECAR VESC odometry estimator nodelet"); - vesc_to_odom_.reset(new VescToOdom(getNodeHandle(), getPrivateNodeHandle())); -} - -} // namespace vesc_ackermann - -PLUGINLIB_EXPORT_CLASS(vesc_ackermann::VescToOdomNodelet, nodelet::Nodelet); diff --git a/vesc_ackermann/vesc_ackermann_nodelet.xml b/vesc_ackermann/vesc_ackermann_nodelet.xml deleted file mode 100644 index b12358a..0000000 --- a/vesc_ackermann/vesc_ackermann_nodelet.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - Nodelet for translating ackermann messages to VESC motor controller commands. - - - Nodelet for translating VESC motor controller state to odometry messages. - - From 45d345aa08a4e660d9c1ac38578988e0d64965da Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 21:58:42 -0600 Subject: [PATCH 12/72] [vesc_ackermann] Porting complete. --- vesc_ackermann/CMakeLists.txt | 1 - .../vesc_ackermann/ackermann_to_vesc.hpp | 65 +++--- .../include/vesc_ackermann/vesc_to_odom.hpp | 80 +++---- .../launch/ackermann_to_vesc_node.launch | 14 -- .../launch/ackermann_to_vesc_node.launch.xml | 14 ++ .../launch/ackermann_to_vesc_nodelet.launch | 29 --- .../launch/vesc_to_odom_node.launch | 14 -- .../launch/vesc_to_odom_node.launch.xml | 19 ++ .../launch/vesc_to_odom_nodelet.launch | 29 --- vesc_ackermann/package.xml | 2 +- vesc_ackermann/src/ackermann_to_vesc.cpp | 101 ++++----- vesc_ackermann/src/vesc_to_odom.cpp | 201 +++++++++--------- 12 files changed, 261 insertions(+), 308 deletions(-) delete mode 100644 vesc_ackermann/launch/ackermann_to_vesc_node.launch create mode 100644 vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml delete mode 100644 vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch delete mode 100644 vesc_ackermann/launch/vesc_to_odom_node.launch create mode 100644 vesc_ackermann/launch/vesc_to_odom_node.launch.xml delete mode 100644 vesc_ackermann/launch/vesc_to_odom_nodelet.launch diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt index 3381102..0940ee0 100644 --- a/vesc_ackermann/CMakeLists.txt +++ b/vesc_ackermann/CMakeLists.txt @@ -33,7 +33,6 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE vesc_to_odom_node ) - ############# ## Testing ## ############# diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp index 33c4d10..b646c36 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp @@ -1,43 +1,50 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ -#define VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ +#ifndef VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ +#define VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ -#include -#include +#include +#include +#include namespace vesc_ackermann { -class AckermannToVesc +using ackermann_msgs::msg::AckermannDriveStamped; +using std_msgs::msg::Float64; + +class AckermannToVesc : public rclcpp::Node { public: - AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh); + explicit AckermannToVesc(const rclcpp::NodeOptions & options); private: // ROS parameters @@ -48,14 +55,14 @@ class AckermannToVesc /** @todo consider also providing an interpolated look-up table conversion */ // ROS services - ros::Publisher erpm_pub_; - ros::Publisher servo_pub_; - ros::Subscriber ackermann_sub_; + rclcpp::Publisher::SharedPtr erpm_pub_; + rclcpp::Publisher::SharedPtr servo_pub_; + rclcpp::Subscription::SharedPtr ackermann_sub_; // ROS callbacks - void ackermannCmdCallback(const ackermann_msgs::AckermannDriveStamped::ConstPtr& cmd); + void ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd); }; } // namespace vesc_ackermann -#endif // VESC_ACKERMANN_ACKERMANN_TO_VESC_H_ +#endif // VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp index f864833..916f2ef 100644 --- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp @@ -1,48 +1,56 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#ifndef VESC_ACKERMANN_VESC_TO_ODOM_H_ -#define VESC_ACKERMANN_VESC_TO_ODOM_H_ +#ifndef VESC_ACKERMANN__VESC_TO_ODOM_HPP_ +#define VESC_ACKERMANN__VESC_TO_ODOM_HPP_ + +#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include - namespace vesc_ackermann { -class VescToOdom +using nav_msgs::msg::Odometry; +using std_msgs::msg::Float64; +using vesc_msgs::msg::VescStateStamped; + +class VescToOdom : public rclcpp::Node { public: - VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh); + explicit VescToOdom(const rclcpp::NodeOptions & options); private: // ROS parameters @@ -58,20 +66,20 @@ class VescToOdom // odometry state double x_, y_, yaw_; - std_msgs::Float64::ConstPtr last_servo_cmd_; ///< Last servo position commanded value - vesc_msgs::VescStateStamped::ConstPtr last_state_; ///< Last received state message + Float64::SharedPtr last_servo_cmd_; ///< Last servo position commanded value + VescStateStamped::SharedPtr 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_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Subscription::SharedPtr vesc_state_sub_; + rclcpp::Subscription::SharedPtr servo_sub_; + std::shared_ptr tf_pub_; // ROS callbacks - void vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state); - void servoCmdCallback(const std_msgs::Float64::ConstPtr& servo); + void vescStateCallback(const VescStateStamped::SharedPtr state); + void servoCmdCallback(const Float64::SharedPtr servo); }; } // namespace vesc_ackermann -#endif // VESC_ACKERMANN_VESC_TO_ODOM_H_ +#endif // VESC_ACKERMANN__VESC_TO_ODOM_HPP_ diff --git a/vesc_ackermann/launch/ackermann_to_vesc_node.launch b/vesc_ackermann/launch/ackermann_to_vesc_node.launch deleted file mode 100644 index d0b2ea9..0000000 --- a/vesc_ackermann/launch/ackermann_to_vesc_node.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml b/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml new file mode 100644 index 0000000..6b7d560 --- /dev/null +++ b/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch b/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch deleted file mode 100644 index 5060fe3..0000000 --- a/vesc_ackermann/launch/ackermann_to_vesc_nodelet.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vesc_ackermann/launch/vesc_to_odom_node.launch b/vesc_ackermann/launch/vesc_to_odom_node.launch deleted file mode 100644 index a2cf374..0000000 --- a/vesc_ackermann/launch/vesc_to_odom_node.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/vesc_ackermann/launch/vesc_to_odom_node.launch.xml b/vesc_ackermann/launch/vesc_to_odom_node.launch.xml new file mode 100644 index 0000000..712d443 --- /dev/null +++ b/vesc_ackermann/launch/vesc_to_odom_node.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/vesc_ackermann/launch/vesc_to_odom_nodelet.launch b/vesc_ackermann/launch/vesc_to_odom_nodelet.launch deleted file mode 100644 index 19a3aab..0000000 --- a/vesc_ackermann/launch/vesc_to_odom_nodelet.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vesc_ackermann/package.xml b/vesc_ackermann/package.xml index ef60194..bd39777 100644 --- a/vesc_ackermann/package.xml +++ b/vesc_ackermann/package.xml @@ -22,7 +22,7 @@ rclcpp rclcpp_components std_msgs - tf2 + tf2_ros vesc_msgs ament_lint_auto diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index da29626..64e8cb0 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -1,91 +1,84 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_ackermann/ackermann_to_vesc.h" +#include "vesc_ackermann/ackermann_to_vesc.hpp" + +#include +#include #include #include #include -#include - namespace vesc_ackermann { -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); +using std::placeholders::_1; -AckermannToVesc::AckermannToVesc(ros::NodeHandle nh, ros::NodeHandle private_nh) +AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) +: Node("ackermann_to_vesc_node", options) { // get conversion parameters - if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) - return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) - return; - if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) - return; - if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) - return; + speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain").get(); + speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset").get(); + steering_to_servo_gain_ = declare_parameter("steering_angle_to_servo_gain").get(); + steering_to_servo_offset_ = declare_parameter("steering_angle_to_servo_offset").get(); // create publishers to vesc electric-RPM (speed) and servo commands - erpm_pub_ = nh.advertise("commands/motor/speed", 10); - servo_pub_ = nh.advertise("commands/servo/position", 10); + erpm_pub_ = create_publisher("commands/motor/speed", 10); + servo_pub_ = create_publisher("commands/servo/position", 10); // subscribe to ackermann topic - ackermann_sub_ = nh.subscribe("ackermann_cmd", 10, &AckermannToVesc::ackermannCmdCallback, this); + ackermann_sub_ = create_subscription( + "ackermann_cmd", 10, std::bind(&AckermannToVesc::ackermannCmdCallback, this, _1)); } -typedef ackermann_msgs::AckermannDriveStamped::ConstPtr AckermannMsgPtr; -void AckermannToVesc::ackermannCmdCallback(const AckermannMsgPtr& cmd) +void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) { // calc vesc electric RPM (speed) - std_msgs::Float64::Ptr erpm_msg(new std_msgs::Float64); - erpm_msg->data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; + Float64 erpm_msg; + erpm_msg.data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; // calc steering angle (servo) - std_msgs::Float64::Ptr servo_msg(new std_msgs::Float64); - servo_msg->data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; + Float64 servo_msg; + servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; // publish - if (ros::ok()) - { - erpm_pub_.publish(erpm_msg); - servo_pub_.publish(servo_msg); + if (rclcpp::ok()) { + erpm_pub_->publish(erpm_msg); + servo_pub_->publish(servo_msg); } } -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) -{ - if (nh.getParam(name, *value)) - return true; +} // namespace vesc_ackermann - ROS_FATAL("AckermannToVesc: Parameter %s is required.", name.c_str()); - return false; -} +#include "rclcpp_components/register_node_macro.hpp" // NOLINT -} // namespace vesc_ackermann +RCLCPP_COMPONENTS_REGISTER_NODE(vesc_ackermann::AckermannToVesc) diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index cca7d11..1744ab0 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -1,188 +1,187 @@ // Copyright 2020 F1TENTH Foundation // -// Redistribution and use in source and binary forms, with or without modification, are permitted -// provided that the following conditions are met: +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: // -// 1. Redistributions of source code must retain the above copyright notice, this list of conditions -// and the following disclaimer. +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. // -// 2. Redistributions in binary form must reproduce the above copyright notice, this list -// of conditions and the following disclaimer in the documentation and/or other materials -// provided with the distribution. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. // -// 3. Neither the name of the copyright holder nor the names of its contributors may be used -// to endorse or promote products derived from this software without specific prior -// written permission. +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. // -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR -// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND -// FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR -// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, -// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY -// WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. // -*- mode:c++; fill-column: 100; -*- -#include "vesc_ackermann/vesc_to_odom.h" +#include "vesc_ackermann/vesc_to_odom.hpp" + +#include #include #include -#include -#include - namespace vesc_ackermann { -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value); - -VescToOdom::VescToOdom(ros::NodeHandle nh, ros::NodeHandle private_nh) : - odom_frame_("odom"), base_frame_("base_link"), - use_servo_cmd_(true), publish_tf_(false), x_(0.0), y_(0.0), yaw_(0.0) +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::Odometry; +using std::placeholders::_1; + +VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) +: Node("vesc_to_odom_node", options), + odom_frame_("odom"), + base_frame_("base_link"), + use_servo_cmd_(true), + publish_tf_(false), + x_(0.0), + y_(0.0), + yaw_(0.0) { // get ROS parameters - private_nh.param("odom_frame", odom_frame_, odom_frame_); - private_nh.param("base_frame", base_frame_, base_frame_); - private_nh.param("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_, use_servo_cmd_); - if (!getRequiredParam(nh, "speed_to_erpm_gain", &speed_to_erpm_gain_)) - return; - if (!getRequiredParam(nh, "speed_to_erpm_offset", &speed_to_erpm_offset_)) - return; - if (use_servo_cmd_) - { - if (!getRequiredParam(nh, "steering_angle_to_servo_gain", &steering_to_servo_gain_)) - return; - if (!getRequiredParam(nh, "steering_angle_to_servo_offset", &steering_to_servo_offset_)) - return; - if (!getRequiredParam(nh, "wheelbase", &wheelbase_)) - return; + declare_parameter("odom_frame", odom_frame_); + declare_parameter("base_frame", base_frame_); + declare_parameter("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_); + + speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain").get(); + speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset").get(); + + if (use_servo_cmd_) { + steering_to_servo_gain_ = declare_parameter("steering_angle_to_servo_gain").get(); + steering_to_servo_offset_ = declare_parameter("steering_angle_to_servo_offset").get(); + wheelbase_ = declare_parameter("wheelbase").get(); } - private_nh.param("publish_tf", publish_tf_, publish_tf_); + + declare_parameter("publish_tf", publish_tf_); // create odom publisher - odom_pub_ = nh.advertise("odom", 10); + odom_pub_ = create_publisher("odom", 10); // create tf broadcaster - if (publish_tf_) - { - tf_pub_.reset(new tf::TransformBroadcaster); + if (publish_tf_) { + tf_pub_.reset(new tf2_ros::TransformBroadcaster(this)); } // subscribe to vesc state and. optionally, servo command - vesc_state_sub_ = nh.subscribe("sensors/core", 10, &VescToOdom::vescStateCallback, this); - if (use_servo_cmd_) - { - servo_sub_ = nh.subscribe("sensors/servo_position_command", 10, - &VescToOdom::servoCmdCallback, this); + vesc_state_sub_ = create_subscription( + "sensors/core", 10, std::bind(&VescToOdom::vescStateCallback, this, _1)); + + if (use_servo_cmd_) { + servo_sub_ = create_subscription( + "sensors/servo_position_command", 10, std::bind(&VescToOdom::servoCmdCallback, this, _1)); } } -void VescToOdom::vescStateCallback(const vesc_msgs::VescStateStamped::ConstPtr& state) +void VescToOdom::vescStateCallback(const VescStateStamped::SharedPtr state) { // check that we have a last servo command if we are depending on it for angular velocity - if (use_servo_cmd_ && !last_servo_cmd_) + if (use_servo_cmd_ && !last_servo_cmd_) { return; + } // convert to engineering units double current_speed = (-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; - if (std::fabs(current_speed) < 0.05) - { + if (std::fabs(current_speed) < 0.05) { current_speed = 0.0; } double current_steering_angle(0.0), current_angular_velocity(0.0); - if (use_servo_cmd_) - { + if (use_servo_cmd_) { current_steering_angle = (last_servo_cmd_->data - steering_to_servo_offset_) / steering_to_servo_gain_; current_angular_velocity = current_speed * tan(current_steering_angle) / wheelbase_; } // use current state as last state if this is our first time here - if (!last_state_) + if (!last_state_) { last_state_ = state; + } // calc elapsed time - ros::Duration dt = state->header.stamp - last_state_->header.stamp; + auto dt = rclcpp::Time(state->header.stamp) - rclcpp::Time(last_state_->header.stamp); /** @todo could probably do better propigating odometry, e.g. trapezoidal integration */ // propigate odometry double x_dot = current_speed * cos(yaw_); double y_dot = current_speed * sin(yaw_); - x_ += x_dot * dt.toSec(); - y_ += y_dot * dt.toSec(); - if (use_servo_cmd_) - yaw_ += current_angular_velocity * dt.toSec(); + x_ += x_dot * dt.seconds(); + y_ += y_dot * dt.seconds(); + if (use_servo_cmd_) { + yaw_ += current_angular_velocity * dt.seconds(); + } // save state for next time last_state_ = state; // publish odometry message - nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry); - odom->header.frame_id = odom_frame_; - odom->header.stamp = state->header.stamp; - odom->child_frame_id = base_frame_; + Odometry odom; + odom.header.frame_id = odom_frame_; + odom.header.stamp = state->header.stamp; + odom.child_frame_id = base_frame_; // Position - odom->pose.pose.position.x = x_; - odom->pose.pose.position.y = y_; - odom->pose.pose.orientation.x = 0.0; - odom->pose.pose.orientation.y = 0.0; - odom->pose.pose.orientation.z = sin(yaw_ / 2.0); - odom->pose.pose.orientation.w = cos(yaw_ / 2.0); + odom.pose.pose.position.x = x_; + odom.pose.pose.position.y = y_; + odom.pose.pose.orientation.x = 0.0; + odom.pose.pose.orientation.y = 0.0; + odom.pose.pose.orientation.z = sin(yaw_ / 2.0); + odom.pose.pose.orientation.w = cos(yaw_ / 2.0); // Position uncertainty /** @todo Think about position uncertainty, perhaps get from parameters? */ - odom->pose.covariance[0] = 0.2; ///< x - odom->pose.covariance[7] = 0.2; ///< y - odom->pose.covariance[35] = 0.4; ///< yaw + odom.pose.covariance[0] = 0.2; ///< x + odom.pose.covariance[7] = 0.2; ///< y + odom.pose.covariance[35] = 0.4; ///< yaw // Velocity ("in the coordinate frame given by the child_frame_id") - odom->twist.twist.linear.x = current_speed; - odom->twist.twist.linear.y = 0.0; - odom->twist.twist.angular.z = current_angular_velocity; + odom.twist.twist.linear.x = current_speed; + odom.twist.twist.linear.y = 0.0; + odom.twist.twist.angular.z = current_angular_velocity; // Velocity uncertainty /** @todo Think about velocity uncertainty */ - if (publish_tf_) - { - geometry_msgs::TransformStamped tf; + if (publish_tf_) { + TransformStamped tf; tf.header.frame_id = odom_frame_; tf.child_frame_id = base_frame_; - tf.header.stamp = ros::Time::now(); + tf.header.stamp = now(); tf.transform.translation.x = x_; tf.transform.translation.y = y_; tf.transform.translation.z = 0.0; - tf.transform.rotation = odom->pose.pose.orientation; - if (ros::ok()) - { + tf.transform.rotation = odom.pose.pose.orientation; + + if (rclcpp::ok()) { tf_pub_->sendTransform(tf); } } - if (ros::ok()) - { - odom_pub_.publish(odom); + if (rclcpp::ok()) { + odom_pub_->publish(odom); } } -void VescToOdom::servoCmdCallback(const std_msgs::Float64::ConstPtr& servo) +void VescToOdom::servoCmdCallback(const Float64::SharedPtr servo) { last_servo_cmd_ = servo; } -template -inline bool getRequiredParam(const ros::NodeHandle& nh, const std::string& name, T* value) -{ - if (nh.getParam(name, *value)) - return true; +} // namespace vesc_ackermann - ROS_FATAL("VescToOdom: Parameter %s is required.", name.c_str()); - return false; -} +#include "rclcpp_components/register_node_macro.hpp" // NOLINT -} // namespace vesc_ackermann +RCLCPP_COMPONENTS_REGISTER_NODE(vesc_ackermann::VescToOdom) From d52e16ae3473fa1a6e1622437a5734b7dcfd84bb Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 22:08:15 -0600 Subject: [PATCH 13/72] [CI] Adding ROS2 build jobs. --- .../workflows/{ros1-ci.yaml => ros2-ci.yaml} | 32 +++++++++++-------- 1 file changed, 19 insertions(+), 13 deletions(-) rename .github/workflows/{ros1-ci.yaml => ros2-ci.yaml} (57%) diff --git a/.github/workflows/ros1-ci.yaml b/.github/workflows/ros2-ci.yaml similarity index 57% rename from .github/workflows/ros1-ci.yaml rename to .github/workflows/ros2-ci.yaml index 056436c..7ff9464 100644 --- a/.github/workflows/ros1-ci.yaml +++ b/.github/workflows/ros2-ci.yaml @@ -1,30 +1,36 @@ -name: ROS1 CI Workflow +name: ROS2 CI Workflow on: pull_request: + branches: + - ros2 push: branches: - - main + - ros2 jobs: - build-ros1: + build-ros2: runs-on: ubuntu-latest strategy: fail-fast: false + matrix: + ros-version: [dashing, foxy] + include: + - ros-version: dashing + image: ros:dashing + - ros-version: foxy + image: ros:foxy container: - image: ros:melodic + image: ${{ matrix.image }} steps: - name: Checkout Repo uses: actions/checkout@v2 - name: Create Workspace run: | - mkdir -p src_tmp/vesc; \ - mv `find -maxdepth 1 -not -name . -not -name src_tmp` src_tmp/vesc/; \ - mv src_tmp/ src/; \ - cd src; \ - bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ - catkin_init_workspace' + mkdir -p src_tmp/vesc + mv `find -maxdepth 1 -not -name . -not -name src_tmp` src_tmp/vesc/ + mv src_tmp/ src/ - name: Install Dependencies run: | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ @@ -33,9 +39,9 @@ jobs: - name: Build Workspace run: | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ - catkin_make' + colcon build --event-handlers console_cohesion+' - name: Run Tests run: | bash -c 'source /opt/ros/$ROS_DISTRO/setup.bash; \ - catkin_make run_tests; \ - catkin_test_results --verbose' + colcon test --event-handlers console_cohesion+; \ + colcon test-result --verbose' From 7dd2e5bca77d7266b6ef808086280e540af299e2 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 22:09:04 -0600 Subject: [PATCH 14/72] [CI] Only running on Foxy. --- .github/workflows/ros2-ci.yaml | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/.github/workflows/ros2-ci.yaml b/.github/workflows/ros2-ci.yaml index 7ff9464..2bb3487 100644 --- a/.github/workflows/ros2-ci.yaml +++ b/.github/workflows/ros2-ci.yaml @@ -12,17 +12,8 @@ jobs: build-ros2: runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - ros-version: [dashing, foxy] - include: - - ros-version: dashing - image: ros:dashing - - ros-version: foxy - image: ros:foxy container: - image: ${{ matrix.image }} + image: ros:foxy steps: - name: Checkout Repo uses: actions/checkout@v2 From 58ff3679148f666300706202f515e6bc719de411 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Sun, 13 Dec 2020 22:10:47 -0600 Subject: [PATCH 15/72] [CI] Updating badge in README. --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 959c0dd..5802dfa 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # Veddar VESC Interface -![ROS1 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS1%20CI%20Workflow/badge.svg) +![ROS2 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS2%20CI%20Workflow/badge.svg) Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details From e2c00b6897b95475bf610df549498a10627292b8 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Wed, 16 Dec 2020 17:51:20 -0600 Subject: [PATCH 16/72] [vesc_driver] Set launch file to typical device for F1TENTH. --- vesc_driver/launch/vesc_driver_node.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/launch/vesc_driver_node.launch.xml b/vesc_driver/launch/vesc_driver_node.launch.xml index b5a730e..cc805dc 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.xml +++ b/vesc_driver/launch/vesc_driver_node.launch.xml @@ -7,7 +7,7 @@ - + From 9f364e7ad0b481119a8997f41dc99b6486ef0dc5 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Wed, 16 Dec 2020 19:07:32 -0600 Subject: [PATCH 17/72] [vesc_driver] Adding all optional params to launch. --- vesc_driver/launch/vesc_driver_node.launch.xml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/vesc_driver/launch/vesc_driver_node.launch.xml b/vesc_driver/launch/vesc_driver_node.launch.xml index cc805dc..3a83355 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.xml +++ b/vesc_driver/launch/vesc_driver_node.launch.xml @@ -7,10 +7,22 @@ - + + + + + + + + + + + + + From 9a50f63fdc7a21043a1f8d50ee8a1725639eb216 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Wed, 16 Dec 2020 19:20:36 -0600 Subject: [PATCH 18/72] [vesc_driver] Re-adding missing join() for thread. --- vesc_driver/src/vesc_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 12c6981..3baf631 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -209,7 +209,7 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread impl_->rx_thread_run_ = false; - // impl_->rx_thread_->join(); + impl_->rx_thread_->join(); impl_->serial_port_.close(); } } From a047acd810d3c59c92066023594873bb0c4fbad3 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Wed, 16 Dec 2020 19:50:08 -0600 Subject: [PATCH 19/72] [vesc_ackermann] Adding some missing include headers and using statements. --- vesc_ackermann/CMakeLists.txt | 2 +- vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml | 6 +++--- vesc_ackermann/src/ackermann_to_vesc.cpp | 2 ++ vesc_ackermann/src/vesc_to_odom.cpp | 2 ++ 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/vesc_ackermann/CMakeLists.txt b/vesc_ackermann/CMakeLists.txt index 0940ee0..9a5ba87 100644 --- a/vesc_ackermann/CMakeLists.txt +++ b/vesc_ackermann/CMakeLists.txt @@ -19,8 +19,8 @@ ament_auto_find_build_dependencies() # nodes library ament_auto_add_library(${PROJECT_NAME} SHARED - src/vesc_to_odom.cpp src/ackermann_to_vesc.cpp + src/vesc_to_odom.cpp ) # register nodes as components diff --git a/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml b/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml index 6b7d560..4cbf7b7 100644 --- a/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml +++ b/vesc_ackermann/launch/ackermann_to_vesc_node.launch.xml @@ -6,9 +6,9 @@ - + - - + + diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 64e8cb0..e26c4f1 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -40,7 +40,9 @@ namespace vesc_ackermann { +using ackermann_msgs::msg::AckermannDriveStamped; using std::placeholders::_1; +using std_msgs::msg::Float64; AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) : Node("ackermann_to_vesc_node", options) diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 1744ab0..1deef6c 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -31,6 +31,7 @@ #include "vesc_ackermann/vesc_to_odom.hpp" #include +#include #include #include @@ -41,6 +42,7 @@ namespace vesc_ackermann using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::Odometry; using std::placeholders::_1; +using vesc_msgs::msg::VescStateStamped; VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) : Node("vesc_to_odom_node", options), From 929b9e534283ee24c3f64e3cd9d9104469c46897 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Tue, 22 Dec 2020 19:00:51 -0600 Subject: [PATCH 20/72] Add mutex and wait in read thread. --- vesc_driver/src/vesc_interface.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 3baf631..4dd65b1 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -61,6 +62,7 @@ class VescInterface::Impl ErrorHandlerFunction error_handler_; boost::asio::io_service io_service_; boost::asio::serial_port serial_port_; + std::mutex serial_mutex_; }; void VescInterface::Impl::rxThread() @@ -126,6 +128,9 @@ void VescInterface::Impl::rxThread() // attempt to read at least bytes_needed bytes from the serial port int bytes_to_read = std::max(bytes_needed, 4096); + + std::lock_guard lock(serial_mutex_); + const size_t bytes_read = boost::asio::read( serial_port_, boost::asio::buffer(buffer, buffer.size()), @@ -134,6 +139,9 @@ void VescInterface::Impl::rxThread() if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); } + + // Only attempt to read every 10 ms + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } } @@ -208,6 +216,7 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread + std::lock_guard lock(impl_->serial_mutex_); impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); impl_->serial_port_.close(); @@ -216,12 +225,14 @@ void VescInterface::disconnect() bool VescInterface::isConnected() const { + std::lock_guard lock(impl_->serial_mutex_); return impl_->serial_port_.is_open(); } void VescInterface::send(const VescPacket & packet) { try { + std::lock_guard lock(impl_->serial_mutex_); size_t written = impl_->serial_port_.write_some( boost::asio::buffer(packet.frame())); if (written != packet.frame().size()) { From 7f49d43f2c51ab6f36fb41d29bd3d0a5b7face5a Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Tue, 22 Dec 2020 19:21:14 -0600 Subject: [PATCH 21/72] [vesc_driver] Changing default port to F1TENTH symlink name. --- vesc_driver/launch/vesc_driver_node.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/launch/vesc_driver_node.launch.xml b/vesc_driver/launch/vesc_driver_node.launch.xml index 3a83355..0e8a8ce 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.xml +++ b/vesc_driver/launch/vesc_driver_node.launch.xml @@ -7,7 +7,7 @@ - + From 69d69993921eb8bfb7191c26ebafaf531ad596d9 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Tue, 22 Dec 2020 19:36:23 -0600 Subject: [PATCH 22/72] [vesc_driver] Optimizing mutex utilization. --- vesc_driver/src/vesc_interface.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 4dd65b1..0b0c20d 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -129,15 +129,17 @@ void VescInterface::Impl::rxThread() // attempt to read at least bytes_needed bytes from the serial port int bytes_to_read = std::max(bytes_needed, 4096); - std::lock_guard lock(serial_mutex_); + { + std::lock_guard lock(serial_mutex_); - const size_t bytes_read = boost::asio::read( - serial_port_, - boost::asio::buffer(buffer, buffer.size()), - boost::asio::transfer_exactly(bytes_to_read)); + const size_t bytes_read = boost::asio::read( + serial_port_, + boost::asio::buffer(buffer, buffer.size()), + boost::asio::transfer_exactly(bytes_to_read)); - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { - error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); + if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { + error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); + } } // Only attempt to read every 10 ms @@ -216,9 +218,10 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread - std::lock_guard lock(impl_->serial_mutex_); impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); + + std::lock_guard lock(impl_->serial_mutex_); impl_->serial_port_.close(); } } From a4dd716af6e757449ca3829a1faae35105713802 Mon Sep 17 00:00:00 2001 From: Haoru Date: Mon, 17 May 2021 02:06:51 +0000 Subject: [PATCH 23/72] working ros2 driver --- README.md | 22 ++ vesc_driver/CMakeLists.txt | 22 +- .../include/vesc_driver/vesc_driver.hpp | 9 +- vesc_driver/package.xml | 3 +- vesc_driver/params/vesc_config.yaml | 15 + vesc_driver/src/vesc_driver.cpp | 6 +- vesc_driver/src/vesc_interface.cpp | 170 +++++----- vesc_driver/src/vesc_interface_old.cpp | 293 ++++++++++++++++++ 8 files changed, 436 insertions(+), 104 deletions(-) create mode 100644 vesc_driver/params/vesc_config.yaml create mode 100644 vesc_driver/src/vesc_interface_old.cpp diff --git a/README.md b/README.md index 5802dfa..ff49614 100644 --- a/README.md +++ b/README.md @@ -3,3 +3,25 @@ ![ROS2 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS2%20CI%20Workflow/badge.svg) Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details + +# Development Log +Haoru Xue 5/16/2021 + +## Completed +- Ported the VESC package to ROS2 +- Sensor reading tested +- Speed and steerign commands tested + +## Potential Improvements +- C++ 17 is currently required to replace `boost::optional` with `std::optional` +- Speed sensor reading is inverted +- Not made LifeCycleNode yet + +## How to test +1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src` folder. +2. `rosdep update && rosdep install --from-paths src -i -y` +3. Plug in the VESC with a USB cable. +4. Check the device name, typically `/dev/ttyACM0`. +5. Modify `vesc/vesc_driver/params/vesc_config.yaml` to reflect any changes. +6. Build the package `colcon build --cmake-args '-DCMAKE_BUILD_TYPE=Debug' --packages-up-to vesc` +7. `ros2 run vesc_driver vesc_driver_node --ros-args -r __node:=vesc_node -r __ns:=/ --params-file /root/ros2_ws/src/vesc/vesc_driver/params/vesc_config.yaml` \ No newline at end of file diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 2d1ca80..23abfb7 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,19 +1,19 @@ cmake_minimum_required(VERSION 3.5) project(vesc_driver) -# Set minimum C++ standard to C++14 -if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++14") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") -elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++14") - set(CMAKE_CXX_STANDARD 14) -endif() - +# Set minimum C++ standard to C++17 +#if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") +# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") +#elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") +# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") +# set(CMAKE_CXX_STANDARD 17) +#endif() +set(CMAKE_CXX_STANDARD 17) find_package(ament_cmake_auto REQUIRED) +find_package(serial_driver REQUIRED) ament_auto_find_build_dependencies() -find_package(Boost COMPONENTS system REQUIRED) find_package(Threads) ########### @@ -28,7 +28,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/vesc_packet_factory.cpp ) target_link_libraries(${PROJECT_NAME} - ${Boost_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ) rclcpp_components_register_node(${PROJECT_NAME} @@ -48,4 +47,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + params ) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index 68f76ce..3a0cfa7 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -31,7 +31,6 @@ #ifndef VESC_DRIVER__VESC_DRIVER_HPP_ #define VESC_DRIVER__VESC_DRIVER_HPP_ -#include #include #include #include @@ -68,14 +67,14 @@ class VescDriver CommandLimit( rclcpp::Node * node_ptr, const std::string & str, - const boost::optional & min_lower = boost::optional(), - const boost::optional & max_upper = boost::optional()); + const std::optional & min_lower = std::optional(), + const std::optional & max_upper = std::optional()); double clip(double value); rclcpp::Node * node_ptr; rclcpp::Logger logger; std::string name; - boost::optional lower; - boost::optional upper; + std::optional lower; + std::optional upper; }; CommandLimit duty_cycle_limit_; diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index c956327..9928817 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -16,12 +16,11 @@ ament_cmake_auto - libboost-dev - libboost-system-dev rclcpp rclcpp_components std_msgs vesc_msgs + serial_driver ament_lint_auto ament_lint_common diff --git a/vesc_driver/params/vesc_config.yaml b/vesc_driver/params/vesc_config.yaml new file mode 100644 index 0000000..98d2a32 --- /dev/null +++ b/vesc_driver/params/vesc_config.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + port: "/dev/ttyACM0" + brake_max: 200000.0 + brake_min: -20000.0 + current_max: 100.0 + current_min: 0.0 + duty_cycle_max: 0.0 + duty_cycle_min: 0.0 + position_max: 0.0 + position_min: 0.0 + servo_max: 0.85 + servo_min: 0.15 + speed_max: 23250.0 + speed_min: -23250.0 diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 07d5760..6b92539 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,7 +65,7 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - std::string port = declare_parameter("port", ""); + std::string port = declare_parameter("port", "/dev/ttyACM0"); // attempt to connect to the serial port try { @@ -266,8 +266,8 @@ void VescDriver::servoCallback(const Float64::SharedPtr servo) VescDriver::CommandLimit::CommandLimit( rclcpp::Node * node_ptr, const std::string & str, - const boost::optional & min_lower, - const boost::optional & max_upper) + const std::optional & min_lower, + const std::optional & max_upper) : node_ptr(node_ptr), logger(node_ptr->get_logger()), name(str) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 0b0c20d..03463f3 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -30,8 +30,8 @@ #include "vesc_driver/vesc_interface.hpp" #include "vesc_driver/vesc_packet_factory.hpp" +#include "serial_driver/serial_driver.hpp" -#include #include #include @@ -50,33 +50,54 @@ class VescInterface::Impl { public: Impl() - : io_service_(), - serial_port_(io_service_) + : owned_ctx{new IoContext(2)}, + serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx)} {} - void rxThread(); + void serial_receive_callback(const std::vector & buffer); + void packet_creation_thread(); + void on_configure(); + void connect(const std::string & port); - std::unique_ptr rx_thread_; - bool rx_thread_run_; + bool packet_thread_run_; + std::unique_ptr packet_thread_; PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; - boost::asio::io_service io_service_; - boost::asio::serial_port serial_port_; - std::mutex serial_mutex_; + std::unique_ptr device_config_; + std::mutex buffer_mutex_; + std::string device_name_; + std::unique_ptr owned_ctx{}; + std::unique_ptr serial_driver_; + + ~Impl(){ + if (owned_ctx) { + owned_ctx->waitForExit(); + } + } +private: + std::vector buffer_; }; -void VescInterface::Impl::rxThread() +void VescInterface::Impl::serial_receive_callback(const std::vector & buffer) { - Buffer buffer; - buffer.reserve(4096); + buffer_mutex_.lock(); // wait untill the current buffer read finishes + buffer_.reserve(buffer_.size() + buffer.size()); + buffer_.insert(buffer_.end(), buffer.begin(), buffer.end()); + buffer_mutex_.unlock(); +} - while (rx_thread_run_) { +void VescInterface::Impl::packet_creation_thread() +{ + while (packet_thread_run_) + { + buffer_mutex_.lock(); int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer.empty()) { + if (!buffer_.empty()){ // search buffer for valid packet(s) - Buffer::iterator iter(buffer.begin()); - Buffer::iterator iter_begin(buffer.begin()); - while (iter != buffer.end()) { + auto iter = buffer_.begin(); + auto iter_begin = buffer_.begin(); + while (iter != buffer_.end()) + { // check if valid start-of-frame character if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) @@ -84,15 +105,15 @@ void VescInterface::Impl::rxThread() // good start, now attempt to create packet std::string error; VescPacketConstPtr packet = - VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); + VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error); if (packet) { // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << - std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } + //if (std::distance(iter_begin, iter) > 0) { + //std::ostringstream ss; + //ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << + // std::distance(iter_begin, iter) << " bytes."; + //error_handler_(ss.str()); + //} // call packet handler packet_handler_(packet); // update state @@ -105,7 +126,7 @@ void VescInterface::Impl::rxThread() break; // for (iter_sof... } else { // else, this was not a packet, move on to next byte - error_handler_(error); + //error_handler_(error); } } @@ -113,40 +134,41 @@ void VescInterface::Impl::rxThread() } // if iter is at the end of the buffer, more bytes are needed - if (iter == buffer.end()) { + if (iter == buffer_.end()) { bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; } // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } - buffer.erase(buffer.begin(), iter); - } - - // attempt to read at least bytes_needed bytes from the serial port - int bytes_to_read = std::max(bytes_needed, 4096); - - { - std::lock_guard lock(serial_mutex_); - - const size_t bytes_read = boost::asio::read( - serial_port_, - boost::asio::buffer(buffer, buffer.size()), - boost::asio::transfer_exactly(bytes_to_read)); - - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { - error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); - } + //if (std::distance(iter_begin, iter) > 0) { + //std::ostringstream ss; + //ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; + //error_handler_(ss.str()); + //} + buffer_.erase(buffer_.begin(), iter); } + buffer_mutex_.unlock(); // Only attempt to read every 10 ms std::this_thread::sleep_for(std::chrono::milliseconds(10)); } } +void VescInterface::Impl::connect(const std::string & port) +{ + uint32_t baud_rate = 115200; + auto fc = drivers::serial_driver::FlowControl::NONE; + auto pt = drivers::serial_driver::Parity::NONE; + auto sb = drivers::serial_driver::StopBits::ONE; + device_config_ = std::make_unique(baud_rate, fc, pt, sb); + serial_driver_->init_port(port, *device_config_); + if (!serial_driver_->port()->is_open()) { + serial_driver_->port()->open(); + serial_driver_->port()->async_receive( + std::bind(&VescInterface::Impl::serial_receive_callback, this, std::placeholders::_1)); + } + +} + VescInterface::VescInterface( const std::string & port, const PacketHandlerFunction & packet_handler, @@ -188,17 +210,7 @@ void VescInterface::connect(const std::string & port) // connect to serial port try { - impl_->serial_port_.open(port); - impl_->serial_port_.set_option(boost::asio::serial_port_base::baud_rate(115200)); - impl_->serial_port_.set_option( - boost::asio::serial_port::flow_control( - boost::asio::serial_port::flow_control::none)); - impl_->serial_port_.set_option( - boost::asio::serial_port::parity( - boost::asio::serial_port::parity::none)); - impl_->serial_port_.set_option( - boost::asio::serial_port::stop_bits( - boost::asio::serial_port::stop_bits::one)); + impl_->connect(port); } catch (const std::exception & e) { std::stringstream ss; ss << "Failed to open the serial port to the VESC. " << e.what(); @@ -206,10 +218,10 @@ void VescInterface::connect(const std::string & port) } // start up a monitoring thread - impl_->rx_thread_run_ = true; - impl_->rx_thread_ = std::unique_ptr( + impl_->packet_thread_run_ = true; + impl_->packet_thread_ = std::unique_ptr( new std::thread( - &VescInterface::Impl::rxThread, impl_.get())); + &VescInterface::Impl::packet_creation_thread, impl_.get())); } void VescInterface::disconnect() @@ -218,36 +230,28 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread - impl_->rx_thread_run_ = false; - impl_->rx_thread_->join(); - - std::lock_guard lock(impl_->serial_mutex_); - impl_->serial_port_.close(); + impl_->packet_thread_run_ = false; + impl_->packet_thread_->join(); + impl_->serial_driver_->port()->close(); } } bool VescInterface::isConnected() const { - std::lock_guard lock(impl_->serial_mutex_); - return impl_->serial_port_.is_open(); + auto port = impl_->serial_driver_->port(); + if (port) + { + return port->is_open(); + } + else + { + return false; + } } void VescInterface::send(const VescPacket & packet) { - try { - std::lock_guard lock(impl_->serial_mutex_); - size_t written = impl_->serial_port_.write_some( - boost::asio::buffer(packet.frame())); - if (written != packet.frame().size()) { - std::stringstream ss; - ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; - throw SerialException(ss.str().c_str()); - } - } catch (const std::exception & e) { - std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); - throw SerialException(ss.str().c_str()); - } + impl_->serial_driver_->port()->async_send(packet.frame()); } void VescInterface::requestFWVersion() diff --git a/vesc_driver/src/vesc_interface_old.cpp b/vesc_driver/src/vesc_interface_old.cpp new file mode 100644 index 0000000..0b0c20d --- /dev/null +++ b/vesc_driver/src/vesc_interface_old.cpp @@ -0,0 +1,293 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// -*- mode:c++; fill-column: 100; -*- + +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vesc_driver +{ + +class VescInterface::Impl +{ +public: + Impl() + : io_service_(), + serial_port_(io_service_) + {} + + void rxThread(); + + std::unique_ptr rx_thread_; + bool rx_thread_run_; + PacketHandlerFunction packet_handler_; + ErrorHandlerFunction error_handler_; + boost::asio::io_service io_service_; + boost::asio::serial_port serial_port_; + std::mutex serial_mutex_; +}; + +void VescInterface::Impl::rxThread() +{ + Buffer buffer; + buffer.reserve(4096); + + while (rx_thread_run_) { + int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + if (!buffer.empty()) { + // search buffer for valid packet(s) + Buffer::iterator iter(buffer.begin()); + Buffer::iterator iter_begin(buffer.begin()); + while (iter != buffer.end()) { + // check if valid start-of-frame character + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || + VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) + { + // good start, now attempt to create packet + std::string error; + VescPacketConstPtr packet = + VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); + if (packet) { + // good packet, check if we skipped any data + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << + std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); + } + // call packet handler + packet_handler_(packet); + // update state + iter = iter + packet->frame().size(); + iter_begin = iter; + // continue to look for another frame in buffer + continue; + } else if (bytes_needed > 0) { + // need more data, break out of while loop + break; // for (iter_sof... + } else { + // else, this was not a packet, move on to next byte + error_handler_(error); + } + } + + iter++; + } + + // if iter is at the end of the buffer, more bytes are needed + if (iter == buffer.end()) { + bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + } + + // erase "used" buffer + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); + } + buffer.erase(buffer.begin(), iter); + } + + // attempt to read at least bytes_needed bytes from the serial port + int bytes_to_read = std::max(bytes_needed, 4096); + + { + std::lock_guard lock(serial_mutex_); + + const size_t bytes_read = boost::asio::read( + serial_port_, + boost::asio::buffer(buffer, buffer.size()), + boost::asio::transfer_exactly(bytes_to_read)); + + if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { + error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); + } + } + + // Only attempt to read every 10 ms + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +VescInterface::VescInterface( + const std::string & port, + const PacketHandlerFunction & packet_handler, + const ErrorHandlerFunction & error_handler) +: impl_(new Impl()) +{ + setPacketHandler(packet_handler); + setErrorHandler(error_handler); + // attempt to conect if the port is specified + if (!port.empty()) { + connect(port); + } +} + +VescInterface::~VescInterface() +{ + disconnect(); +} + +void VescInterface::setPacketHandler(const PacketHandlerFunction & handler) +{ + // todo - definately need mutex + impl_->packet_handler_ = handler; +} + +void VescInterface::setErrorHandler(const ErrorHandlerFunction & handler) +{ + // todo - definately need mutex + impl_->error_handler_ = handler; +} + +void VescInterface::connect(const std::string & port) +{ + // todo - mutex? + + if (isConnected()) { + throw SerialException("Already connected to serial port."); + } + + // connect to serial port + try { + impl_->serial_port_.open(port); + impl_->serial_port_.set_option(boost::asio::serial_port_base::baud_rate(115200)); + impl_->serial_port_.set_option( + boost::asio::serial_port::flow_control( + boost::asio::serial_port::flow_control::none)); + impl_->serial_port_.set_option( + boost::asio::serial_port::parity( + boost::asio::serial_port::parity::none)); + impl_->serial_port_.set_option( + boost::asio::serial_port::stop_bits( + boost::asio::serial_port::stop_bits::one)); + } catch (const std::exception & e) { + std::stringstream ss; + ss << "Failed to open the serial port to the VESC. " << e.what(); + throw SerialException(ss.str().c_str()); + } + + // start up a monitoring thread + impl_->rx_thread_run_ = true; + impl_->rx_thread_ = std::unique_ptr( + new std::thread( + &VescInterface::Impl::rxThread, impl_.get())); +} + +void VescInterface::disconnect() +{ + // todo - mutex? + + if (isConnected()) { + // bring down read thread + impl_->rx_thread_run_ = false; + impl_->rx_thread_->join(); + + std::lock_guard lock(impl_->serial_mutex_); + impl_->serial_port_.close(); + } +} + +bool VescInterface::isConnected() const +{ + std::lock_guard lock(impl_->serial_mutex_); + return impl_->serial_port_.is_open(); +} + +void VescInterface::send(const VescPacket & packet) +{ + try { + std::lock_guard lock(impl_->serial_mutex_); + size_t written = impl_->serial_port_.write_some( + boost::asio::buffer(packet.frame())); + if (written != packet.frame().size()) { + std::stringstream ss; + ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; + throw SerialException(ss.str().c_str()); + } + } catch (const std::exception & e) { + std::stringstream ss; + ss << "Failed to open the serial port to the VESC. " << e.what(); + throw SerialException(ss.str().c_str()); + } +} + +void VescInterface::requestFWVersion() +{ + send(VescPacketRequestFWVersion()); +} + +void VescInterface::requestState() +{ + send(VescPacketRequestValues()); +} + +void VescInterface::setDutyCycle(double duty_cycle) +{ + send(VescPacketSetDuty(duty_cycle)); +} + +void VescInterface::setCurrent(double current) +{ + send(VescPacketSetCurrent(current)); +} + +void VescInterface::setBrake(double brake) +{ + send(VescPacketSetCurrentBrake(brake)); +} + +void VescInterface::setSpeed(double speed) +{ + send(VescPacketSetRPM(speed)); +} + +void VescInterface::setPosition(double position) +{ + send(VescPacketSetPos(position)); +} + +void VescInterface::setServo(double servo) +{ + send(VescPacketSetServoPos(servo)); +} + +} // namespace vesc_driver From 255e0e705d087382da22077e2da309522fce4f53 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Sat, 22 May 2021 12:46:47 -0700 Subject: [PATCH 24/72] resolved inbound serial comunication problems --- vesc_driver/src/vesc_driver.cpp | 5 +++- vesc_driver/src/vesc_interface.cpp | 38 +++++++++++++++++++++++------- 2 files changed, 34 insertions(+), 9 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 07d5760..d5dc0cf 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,7 +65,10 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - std::string port = declare_parameter("port", ""); + // default to /tmp/ttyV0 for debug purpose + // use socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' + // to sniff comunication data to and from vesc + std::string port = declare_parameter("port", "/tmp/ttyV0"); // attempt to connect to the serial port try { diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 0b0c20d..b56cd5d 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -67,12 +67,18 @@ class VescInterface::Impl void VescInterface::Impl::rxThread() { + //the size varies dynamically and start from 0. capacity is 4096 fixed. Buffer buffer; buffer.reserve(4096); + //buffer with fixed size used to read from serial + Buffer bufferRx(4096); + while (rx_thread_run_) { int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + //std::cout << "loop " << bytes_needed < lock(serial_mutex_); + // removed temporarily because it causes a deadlock + //std::lock_guard lock(serial_mutex_); + boost::system::error_code ec; const size_t bytes_read = boost::asio::read( serial_port_, - boost::asio::buffer(buffer, buffer.size()), - boost::asio::transfer_exactly(bytes_to_read)); + boost::asio::buffer(bufferRx, bufferRx.size()), + boost::asio::transfer_exactly(bytes_to_read), + ec + ); + + if (ec.value() > 0 ){ + std::ostringstream ss; + ss << "Serial port comunication error "<< ec << ec < 0 && 0 == bytes_read && !buffer.empty()) { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); @@ -201,7 +223,7 @@ void VescInterface::connect(const std::string & port) boost::asio::serial_port::stop_bits::one)); } catch (const std::exception & e) { std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); + ss << "Failed to open the serial port "<< port <<" to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } @@ -221,21 +243,21 @@ void VescInterface::disconnect() impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); - std::lock_guard lock(impl_->serial_mutex_); + //std::lock_guard lock(impl_->serial_mutex_); impl_->serial_port_.close(); } } bool VescInterface::isConnected() const { - std::lock_guard lock(impl_->serial_mutex_); + //std::lock_guard lock(impl_->serial_mutex_); return impl_->serial_port_.is_open(); } void VescInterface::send(const VescPacket & packet) { try { - std::lock_guard lock(impl_->serial_mutex_); + //std::lock_guard lock(impl_->serial_mutex_); size_t written = impl_->serial_port_.write_some( boost::asio::buffer(packet.frame())); if (written != packet.frame().size()) { From 2a00e4ceece84893884512f4fb8724bf86d02774 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Sat, 22 May 2021 13:31:52 -0700 Subject: [PATCH 25/72] protocol update to FW 5.2 --- vesc_driver/include/vesc_driver/datatypes.hpp | 1107 ++++++++++++++--- .../include/vesc_driver/vesc_packet.hpp | 62 +- vesc_driver/src/vesc_driver.cpp | 40 +- vesc_driver/src/vesc_packet.cpp | 275 +++- vesc_msgs/msg/VescState.msg | 37 +- 5 files changed, 1276 insertions(+), 245 deletions(-) diff --git a/vesc_driver/include/vesc_driver/datatypes.hpp b/vesc_driver/include/vesc_driver/datatypes.hpp index e195182..c3043e4 100644 --- a/vesc_driver/include/vesc_driver/datatypes.hpp +++ b/vesc_driver/include/vesc_driver/datatypes.hpp @@ -28,24 +28,29 @@ /* - Some parts of this code, Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se + Some parts of this code, Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se - VESC Tool is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. + This file is part of the VESC firmware. - VESC Tool is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. -#ifndef VESC_DRIVER__DATATYPES_HPP_ -#define VESC_DRIVER__DATATYPES_HPP_ + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef DATATYPES_H_ +#define DATATYPES_H_ + +#include +#include #include @@ -81,139 +86,961 @@ typedef enum VESC_TX_DOUBLE16, VESC_TX_DOUBLE32, VESC_TX_DOUBLE32_AUTO -} -VESC_TX_T; +} VESC_TX_T; -typedef enum -{ - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR -} -mc_fault_code; +//#include "ch.h" +typedef uint32_t systime_t; // defined in ch.h -typedef enum -{ - DISP_POS_MODE_NONE = 0, - DISP_POS_MODE_INDUCTANCE, - DISP_POS_MODE_OBSERVER, - DISP_POS_MODE_ENCODER, - DISP_POS_MODE_PID_POS, - DISP_POS_MODE_PID_POS_ERROR, - DISP_POS_MODE_ENCODER_OBSERVER_ERROR -} -disp_pos_mode; +// Data types +typedef enum { + MC_STATE_OFF = 0, + MC_STATE_DETECTING, + MC_STATE_RUNNING, + MC_STATE_FULL_BRAKE, +} mc_state; -struct MC_VALUES -{ -public: - double v_in; - double temp_mos; - double temp_motor; - double current_motor; - double current_in; - double id; - double iq; - double rpm; - double duty_now; - double amp_hours; - double amp_hours_charged; - double watt_hours; - double watt_hours_charged; - int tachometer; - int tachometer_abs; - double position; - mc_fault_code fault_code; -}; +typedef enum { + PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended + PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode + PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs +} mc_pwm_mode; -typedef enum -{ - DEBUG_SAMPLING_OFF = 0, - DEBUG_SAMPLING_NOW, - DEBUG_SAMPLING_START, - DEBUG_SAMPLING_TRIGGER_START, - DEBUG_SAMPLING_TRIGGER_FAULT, - DEBUG_SAMPLING_TRIGGER_START_NOSEND, - DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, - DEBUG_SAMPLING_SEND_LAST_SAMPLES -} -debug_sampling_mode; +typedef enum { + COMM_MODE_INTEGRATE = 0, + COMM_MODE_DELAY +} mc_comm_mode; -typedef enum -{ - COMM_FW_VERSION = 0, - COMM_JUMP_TO_BOOTLOADER, - COMM_ERASE_NEW_APP, - COMM_WRITE_NEW_APP_DATA, - COMM_GET_VALUES, - COMM_SET_DUTY, - COMM_SET_CURRENT, - COMM_SET_CURRENT_BRAKE, - COMM_SET_RPM, - COMM_SET_POS, - COMM_SET_HANDBRAKE, - COMM_SET_DETECT, - COMM_SET_SERVO_POS, - COMM_SET_MCCONF, - COMM_GET_MCCONF, - COMM_GET_MCCONF_DEFAULT, - COMM_SET_APPCONF, - COMM_GET_APPCONF, - COMM_GET_APPCONF_DEFAULT, - COMM_SAMPLE_PRINT, - COMM_TERMINAL_CMD, - COMM_PRINT, - COMM_ROTOR_POSITION, - COMM_EXPERIMENT_SAMPLE, - COMM_DETECT_MOTOR_PARAM, - COMM_DETECT_MOTOR_R_L, - COMM_DETECT_MOTOR_FLUX_LINKAGE, - COMM_DETECT_ENCODER, - COMM_DETECT_HALL_FOC, - COMM_REBOOT, - COMM_ALIVE, - COMM_GET_DECODED_PPM, - COMM_GET_DECODED_ADC, - COMM_GET_DECODED_CHUK, - COMM_FORWARD_CAN, - COMM_SET_CHUCK_DATA, - COMM_CUSTOM_APP_DATA, - COMM_NRF_START_PAIRING -} -COMM_PACKET_ID; +typedef enum { + SENSOR_MODE_SENSORLESS = 0, + SENSOR_MODE_SENSORED, + SENSOR_MODE_HYBRID +} mc_sensor_mode; -typedef struct -{ - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; -} -chuck_data; +typedef enum { + FOC_SENSOR_MODE_SENSORLESS = 0, + FOC_SENSOR_MODE_ENCODER, + FOC_SENSOR_MODE_HALL, + FOC_SENSOR_MODE_HFI +} mc_foc_sensor_mode; -struct bldc_detect -{ -public: - double cycle_int_limit; - double bemf_coupling_k; - int hall_res; -}; +// Auxiliary output mode +typedef enum { + OUT_AUX_MODE_OFF = 0, + OUT_AUX_MODE_ON_AFTER_2S, + OUT_AUX_MODE_ON_AFTER_5S, + OUT_AUX_MODE_ON_AFTER_10S, + OUT_AUX_MODE_UNUSED +} out_aux_mode; -typedef enum -{ - NRF_PAIR_STARTED = 0, - NRF_PAIR_OK, - NRF_PAIR_FAIL -} -NRF_PAIR_RES; +// Temperature sensor type +typedef enum { + TEMP_SENSOR_NTC_10K_25C = 0, + TEMP_SENSOR_PTC_1K_100C, + TEMP_SENSOR_KTY83_122, +} temp_sensor_type; + +// General purpose drive output mode +typedef enum { + GPD_OUTPUT_MODE_NONE = 0, + GPD_OUTPUT_MODE_MODULATION, + GPD_OUTPUT_MODE_VOLTAGE, + GPD_OUTPUT_MODE_CURRENT +} gpd_output_mode; + +typedef enum { + MOTOR_TYPE_BLDC = 0, + MOTOR_TYPE_DC, + MOTOR_TYPE_FOC, + MOTOR_TYPE_GPD +} mc_motor_type; + +// FOC current controller decoupling mode. +typedef enum { + FOC_CC_DECOUPLING_DISABLED = 0, + FOC_CC_DECOUPLING_CROSS, + FOC_CC_DECOUPLING_BEMF, + FOC_CC_DECOUPLING_CROSS_BEMF +} mc_foc_cc_decoupling_mode; + +typedef enum { + FOC_OBSERVER_ORTEGA_ORIGINAL = 0, + FOC_OBSERVER_ORTEGA_ITERATIVE +} mc_foc_observer_type; + +typedef enum { + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR, + FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, + FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, + FAULT_CODE_MCU_UNDER_VOLTAGE, + FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, + FAULT_CODE_ENCODER_SPI, + FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, + FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, + FAULT_CODE_FLASH_CORRUPTION, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, + FAULT_CODE_UNBALANCED_CURRENTS, + FAULT_CODE_BRK, + FAULT_CODE_RESOLVER_LOT, + FAULT_CODE_RESOLVER_DOS, + FAULT_CODE_RESOLVER_LOS +} mc_fault_code; + +typedef enum { + CONTROL_MODE_DUTY = 0, + CONTROL_MODE_SPEED, + CONTROL_MODE_CURRENT, + CONTROL_MODE_CURRENT_BRAKE, + CONTROL_MODE_POS, + CONTROL_MODE_HANDBRAKE, + CONTROL_MODE_OPENLOOP, + CONTROL_MODE_OPENLOOP_PHASE, + CONTROL_MODE_OPENLOOP_DUTY, + CONTROL_MODE_OPENLOOP_DUTY_PHASE, + CONTROL_MODE_NONE +} mc_control_mode; + +typedef enum { + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR +} disp_pos_mode; + +typedef enum { + SENSOR_PORT_MODE_HALL = 0, + SENSOR_PORT_MODE_ABI, + SENSOR_PORT_MODE_AS5047_SPI, + SENSOR_PORT_MODE_AD2S1205, + SENSOR_PORT_MODE_SINCOS, + SENSOR_PORT_MODE_TS5700N8501, + SENSOR_PORT_MODE_TS5700N8501_MULTITURN +} sensor_port_mode; + +typedef struct { + float cycle_int_limit; + float cycle_int_limit_running; + float cycle_int_limit_max; + float comm_time_sum; + float comm_time_sum_min_rpm; + int32_t comms; + float time_at_comm; +} mc_rpm_dep_struct; + +typedef enum { + DRV8301_OC_LIMIT = 0, + DRV8301_OC_LATCH_SHUTDOWN, + DRV8301_OC_REPORT_ONLY, + DRV8301_OC_DISABLED +} drv8301_oc_mode; + +typedef enum { + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES +} debug_sampling_mode; + +typedef enum { + CAN_BAUD_125K = 0, + CAN_BAUD_250K, + CAN_BAUD_500K, + CAN_BAUD_1M, + CAN_BAUD_10K, + CAN_BAUD_20K, + CAN_BAUD_50K, + CAN_BAUD_75K +} CAN_BAUD; + +typedef enum { + BATTERY_TYPE_LIION_3_0__4_2, + BATTERY_TYPE_LIIRON_2_6__3_6, + BATTERY_TYPE_LEAD_ACID +} BATTERY_TYPE; + +typedef enum { + HFI_SAMPLES_8 = 0, + HFI_SAMPLES_16, + HFI_SAMPLES_32 +} FOC_HFI_SAMPLES; + +typedef struct { + // Switching and drive + mc_pwm_mode pwm_mode; + mc_comm_mode comm_mode; + mc_motor_type motor_type; + mc_sensor_mode sensor_mode; + // Limits + float l_current_max; + float l_current_min; + float l_in_current_max; + float l_in_current_min; + float l_abs_current_max; + float l_min_erpm; + float l_max_erpm; + float l_erpm_start; + float l_max_erpm_fbrake; + float l_max_erpm_fbrake_cc; + float l_min_vin; + float l_max_vin; + float l_battery_cut_start; + float l_battery_cut_end; + bool l_slow_abs_current; + float l_temp_fet_start; + float l_temp_fet_end; + float l_temp_motor_start; + float l_temp_motor_end; + float l_temp_accel_dec; + float l_min_duty; + float l_max_duty; + float l_watt_max; + float l_watt_min; + float l_current_max_scale; + float l_current_min_scale; + float l_duty_start; + // Overridden limits (Computed during runtime) + float lo_current_max; + float lo_current_min; + float lo_in_current_max; + float lo_in_current_min; + float lo_current_motor_max_now; + float lo_current_motor_min_now; + // Sensorless (bldc) + float sl_min_erpm; + float sl_min_erpm_cycle_int_limit; + float sl_max_fullbreak_current_dir_change; + float sl_cycle_int_limit; + float sl_phase_advance_at_br; + float sl_cycle_int_rpm_br; + float sl_bemf_coupling_k; + // Hall sensor + int8_t hall_table[8]; + float hall_sl_erpm; + // FOC + float foc_current_kp; + float foc_current_ki; + float foc_f_sw; + float foc_dt_us; + float foc_encoder_offset; + bool foc_encoder_inverted; + float foc_encoder_ratio; + float foc_encoder_sin_offset; + float foc_encoder_sin_gain; + float foc_encoder_cos_offset; + float foc_encoder_cos_gain; + float foc_encoder_sincos_filter_constant; + float foc_motor_l; + float foc_motor_r; + float foc_motor_flux_linkage; + float foc_observer_gain; + float foc_observer_gain_slow; + float foc_pll_kp; + float foc_pll_ki; + float foc_duty_dowmramp_kp; + float foc_duty_dowmramp_ki; + float foc_openloop_rpm; + float foc_sl_openloop_hyst; + float foc_sl_openloop_time; + float foc_sl_d_current_duty; + float foc_sl_d_current_factor; + mc_foc_sensor_mode foc_sensor_mode; + uint8_t foc_hall_table[8]; + float foc_sl_erpm; + bool foc_sample_v0_v7; + bool foc_sample_high_current; + float foc_sat_comp; + bool foc_temp_comp; + float foc_temp_comp_base_temp; + float foc_current_filter_const; + mc_foc_cc_decoupling_mode foc_cc_decoupling; + mc_foc_observer_type foc_observer_type; + float foc_hfi_voltage_start; + float foc_hfi_voltage_run; + float foc_hfi_voltage_max; + float foc_sl_erpm_hfi; + uint16_t foc_hfi_start_samples; + float foc_hfi_obs_ovr_sec; + FOC_HFI_SAMPLES foc_hfi_samples; + // GPDrive + int gpd_buffer_notify_left; + int gpd_buffer_interpol; + float gpd_current_filter_const; + float gpd_current_kp; + float gpd_current_ki; + // Speed PID + float s_pid_kp; + float s_pid_ki; + float s_pid_kd; + float s_pid_kd_filter; + float s_pid_min_erpm; + bool s_pid_allow_braking; + // Pos PID + float p_pid_kp; + float p_pid_ki; + float p_pid_kd; + float p_pid_kd_filter; + float p_pid_ang_div; + // Current controller + float cc_startup_boost_duty; + float cc_min_current; + float cc_gain; + float cc_ramp_step_max; + // Misc + int32_t m_fault_stop_time_ms; + float m_duty_ramp_step; + float m_current_backoff_gain; + uint32_t m_encoder_counts; + sensor_port_mode m_sensor_port_mode; + bool m_invert_direction; + drv8301_oc_mode m_drv8301_oc_mode; + int m_drv8301_oc_adj; + float m_bldc_f_sw_min; + float m_bldc_f_sw_max; + float m_dc_f_sw; + float m_ntc_motor_beta; + out_aux_mode m_out_aux_mode; + temp_sensor_type m_motor_temp_sens_type; + float m_ptc_motor_coeff; + // Setup info + uint8_t si_motor_poles; + float si_gear_ratio; + float si_wheel_diameter; + BATTERY_TYPE si_battery_type; + int si_battery_cells; + float si_battery_ah; +} mc_configuration; + +// Applications to use +typedef enum { + APP_NONE = 0, + APP_PPM, + APP_ADC, + APP_UART, + APP_PPM_UART, + APP_ADC_UART, + APP_NUNCHUK, + APP_NRF, + APP_CUSTOM, + APP_BALANCE +} app_use; + +// Throttle curve mode +typedef enum { + THR_EXP_EXPO = 0, + THR_EXP_NATURAL, + THR_EXP_POLY +} thr_exp_mode; + +// PPM control types +typedef enum { + PPM_CTRL_TYPE_NONE = 0, + PPM_CTRL_TYPE_CURRENT, + PPM_CTRL_TYPE_CURRENT_NOREV, + PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, + PPM_CTRL_TYPE_DUTY, + PPM_CTRL_TYPE_DUTY_NOREV, + PPM_CTRL_TYPE_PID, + PPM_CTRL_TYPE_PID_NOREV, + PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST, + PPM_CTRL_TYPE_CURRENT_SMART_REV +} ppm_control_type; + +typedef struct { + ppm_control_type ctrl_type; + float pid_max_erpm; + float hyst; + float pulse_start; + float pulse_end; + float pulse_center; + bool median_filter; + bool safe_start; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + float max_erpm_for_dir; + float smart_rev_max_duty; + float smart_rev_ramp_time; +} ppm_config; + +// ADC control types +typedef enum { + ADC_CTRL_TYPE_NONE = 0, + ADC_CTRL_TYPE_CURRENT, + ADC_CTRL_TYPE_CURRENT_REV_CENTER, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, + ADC_CTRL_TYPE_DUTY, + ADC_CTRL_TYPE_DUTY_REV_CENTER, + ADC_CTRL_TYPE_DUTY_REV_BUTTON, + ADC_CTRL_TYPE_PID, + ADC_CTRL_TYPE_PID_REV_CENTER, + ADC_CTRL_TYPE_PID_REV_BUTTON +} adc_control_type; -} // namespace vesc_driver +typedef struct { + adc_control_type ctrl_type; + float hyst; + float voltage_start; + float voltage_end; + float voltage_center; + float voltage2_start; + float voltage2_end; + bool use_filter; + bool safe_start; + bool cc_button_inverted; + bool rev_button_inverted; + bool voltage_inverted; + bool voltage2_inverted; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + uint32_t update_rate_hz; +} adc_config; -#endif // VESC_DRIVER__DATATYPES_HPP_ +// Nunchuk control types +typedef enum { + CHUK_CTRL_TYPE_NONE = 0, + CHUK_CTRL_TYPE_CURRENT, + CHUK_CTRL_TYPE_CURRENT_NOREV +} chuk_control_type; + +typedef struct { + chuk_control_type ctrl_type; + float hyst; + float ramp_time_pos; + float ramp_time_neg; + float stick_erpm_per_s_in_cc; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + bool multi_esc; + bool tc; + float tc_max_diff; + bool use_smart_rev; + float smart_rev_max_duty; + float smart_rev_ramp_time; +} chuk_config; + +// NRF Datatypes +typedef enum { + NRF_SPEED_250K = 0, + NRF_SPEED_1M, + NRF_SPEED_2M +} NRF_SPEED; + +typedef enum { + NRF_POWER_M18DBM = 0, + NRF_POWER_M12DBM, + NRF_POWER_M6DBM, + NRF_POWER_0DBM, + NRF_POWER_OFF +} NRF_POWER; + +typedef enum { + NRF_AW_3 = 0, + NRF_AW_4, + NRF_AW_5 +} NRF_AW; + +typedef enum { + NRF_CRC_DISABLED = 0, + NRF_CRC_1B, + NRF_CRC_2B +} NRF_CRC; + +typedef enum { + NRF_RETR_DELAY_250US = 0, + NRF_RETR_DELAY_500US, + NRF_RETR_DELAY_750US, + NRF_RETR_DELAY_1000US, + NRF_RETR_DELAY_1250US, + NRF_RETR_DELAY_1500US, + NRF_RETR_DELAY_1750US, + NRF_RETR_DELAY_2000US, + NRF_RETR_DELAY_2250US, + NRF_RETR_DELAY_2500US, + NRF_RETR_DELAY_2750US, + NRF_RETR_DELAY_3000US, + NRF_RETR_DELAY_3250US, + NRF_RETR_DELAY_3500US, + NRF_RETR_DELAY_3750US, + NRF_RETR_DELAY_4000US +} NRF_RETR_DELAY; + +typedef struct { + NRF_SPEED speed; + NRF_POWER power; + NRF_CRC crc_type; + NRF_RETR_DELAY retry_delay; + unsigned char retries; + unsigned char channel; + unsigned char address[3]; + bool send_crc_ack; +} nrf_config; + +typedef struct { + float kp; + float ki; + float kd; + uint16_t hertz; + float pitch_fault; + float roll_fault; + float adc1; + float adc2; + float overspeed_duty; + float tiltback_duty; + float tiltback_angle; + float tiltback_speed; + float tiltback_high_voltage; + float tiltback_low_voltage; + float startup_pitch_tolerance; + float startup_roll_tolerance; + float startup_speed; + float deadzone; + float current_boost; + bool multi_esc; + float yaw_kp; + float yaw_ki; + float yaw_kd; + float roll_steer_kp; + float brake_current; + uint16_t overspeed_delay; + uint16_t fault_delay; + float tiltback_constant; + float roll_steer_erpm_kp; + float yaw_current_clamp; + uint16_t adc_half_fault_erpm; + float setpoint_pitch_filter; + float setpoint_target_filter; + float setpoint_clamp; +} balance_config; + +// CAN status modes +typedef enum { + CAN_STATUS_DISABLED = 0, + CAN_STATUS_1, + CAN_STATUS_1_2, + CAN_STATUS_1_2_3, + CAN_STATUS_1_2_3_4, + CAN_STATUS_1_2_3_4_5 +} CAN_STATUS_MODE; + +typedef enum { + SHUTDOWN_MODE_ALWAYS_OFF = 0, + SHUTDOWN_MODE_ALWAYS_ON, + SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY, + SHUTDOWN_MODE_OFF_AFTER_10S, + SHUTDOWN_MODE_OFF_AFTER_1M, + SHUTDOWN_MODE_OFF_AFTER_5M, + SHUTDOWN_MODE_OFF_AFTER_10M, + SHUTDOWN_MODE_OFF_AFTER_30M, + SHUTDOWN_MODE_OFF_AFTER_1H, + SHUTDOWN_MODE_OFF_AFTER_5H, +} SHUTDOWN_MODE; + +typedef enum { + IMU_TYPE_OFF = 0, + IMU_TYPE_INTERNAL, + IMU_TYPE_EXTERNAL_MPU9X50, + IMU_TYPE_EXTERNAL_ICM20948, + IMU_TYPE_EXTERNAL_BMI160 +} IMU_TYPE; + +typedef enum { + AHRS_MODE_MADGWICK = 0, + AHRS_MODE_MAHONY +} AHRS_MODE; + +typedef struct { + IMU_TYPE type; + AHRS_MODE mode; + int sample_rate_hz; + float accel_confidence_decay; + float mahony_kp; + float mahony_ki; + float madgwick_beta; + float rot_roll; + float rot_pitch; + float rot_yaw; + float accel_offsets[3]; + float gyro_offsets[3]; + float gyro_offset_comp_fact[3]; + float gyro_offset_comp_clamp; +} imu_config; + +typedef enum { + CAN_MODE_VESC = 0, + CAN_MODE_UAVCAN, + CAN_MODE_COMM_BRIDGE +} CAN_MODE; + +typedef struct { + // Settings + uint8_t controller_id; + uint32_t timeout_msec; + float timeout_brake_current; + CAN_STATUS_MODE send_can_status; + uint32_t send_can_status_rate_hz; + CAN_BAUD can_baud_rate; + bool pairing_done; + bool permanent_uart_enabled; + SHUTDOWN_MODE shutdown_mode; + + // CAN modes + CAN_MODE can_mode; + uint8_t uavcan_esc_index; + + // Application to use + app_use app_to_use; + + // PPM application settings + ppm_config app_ppm_conf; + + // ADC application settings + adc_config app_adc_conf; + + // UART application settings + uint32_t app_uart_baudrate; + + // Nunchuk application settings + chuk_config app_chuk_conf; + + // NRF application settings + nrf_config app_nrf_conf; + + // Balance application settings + balance_config app_balance_conf; + + // IMU Settings + imu_config imu_conf; +} app_configuration; + +// Communication commands +typedef enum { + COMM_FW_VERSION = 0, + COMM_JUMP_TO_BOOTLOADER, + COMM_ERASE_NEW_APP, + COMM_WRITE_NEW_APP_DATA, + COMM_GET_VALUES, + COMM_SET_DUTY, + COMM_SET_CURRENT, + COMM_SET_CURRENT_BRAKE, + COMM_SET_RPM, + COMM_SET_POS, + COMM_SET_HANDBRAKE, + COMM_SET_DETECT, + COMM_SET_SERVO_POS, + COMM_SET_MCCONF, + COMM_GET_MCCONF, + COMM_GET_MCCONF_DEFAULT, + COMM_SET_APPCONF, + COMM_GET_APPCONF, + COMM_GET_APPCONF_DEFAULT, + COMM_SAMPLE_PRINT, + COMM_TERMINAL_CMD, + COMM_PRINT, + COMM_ROTOR_POSITION, + COMM_EXPERIMENT_SAMPLE, + COMM_DETECT_MOTOR_PARAM, + COMM_DETECT_MOTOR_R_L, + COMM_DETECT_MOTOR_FLUX_LINKAGE, + COMM_DETECT_ENCODER, + COMM_DETECT_HALL_FOC, + COMM_REBOOT, + COMM_ALIVE, + COMM_GET_DECODED_PPM, + COMM_GET_DECODED_ADC, + COMM_GET_DECODED_CHUK, + COMM_FORWARD_CAN, + COMM_SET_CHUCK_DATA, + COMM_CUSTOM_APP_DATA, + COMM_NRF_START_PAIRING, + COMM_GPD_SET_FSW, + COMM_GPD_BUFFER_NOTIFY, + COMM_GPD_BUFFER_SIZE_LEFT, + COMM_GPD_FILL_BUFFER, + COMM_GPD_OUTPUT_SAMPLE, + COMM_GPD_SET_MODE, + COMM_GPD_FILL_BUFFER_INT8, + COMM_GPD_FILL_BUFFER_INT16, + COMM_GPD_SET_BUFFER_INT_SCALE, + COMM_GET_VALUES_SETUP, + COMM_SET_MCCONF_TEMP, + COMM_SET_MCCONF_TEMP_SETUP, + COMM_GET_VALUES_SELECTIVE, + COMM_GET_VALUES_SETUP_SELECTIVE, + COMM_EXT_NRF_PRESENT, + COMM_EXT_NRF_ESB_SET_CH_ADDR, + COMM_EXT_NRF_ESB_SEND_DATA, + COMM_EXT_NRF_ESB_RX_DATA, + COMM_EXT_NRF_SET_ENABLED, + COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + COMM_DETECT_APPLY_ALL_FOC, + COMM_JUMP_TO_BOOTLOADER_ALL_CAN, + COMM_ERASE_NEW_APP_ALL_CAN, + COMM_WRITE_NEW_APP_DATA_ALL_CAN, + COMM_PING_CAN, + COMM_APP_DISABLE_OUTPUT, + COMM_TERMINAL_CMD_SYNC, + COMM_GET_IMU_DATA, + COMM_BM_CONNECT, + COMM_BM_ERASE_FLASH_ALL, + COMM_BM_WRITE_FLASH, + COMM_BM_REBOOT, + COMM_BM_DISCONNECT, + COMM_BM_MAP_PINS_DEFAULT, + COMM_BM_MAP_PINS_NRF5X, + COMM_ERASE_BOOTLOADER, + COMM_ERASE_BOOTLOADER_ALL_CAN, + COMM_PLOT_INIT, + COMM_PLOT_DATA, + COMM_PLOT_ADD_GRAPH, + COMM_PLOT_SET_GRAPH, + COMM_GET_DECODED_BALANCE, + COMM_BM_MEM_READ, + COMM_WRITE_NEW_APP_DATA_LZO, + COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, + COMM_BM_WRITE_FLASH_LZO, + COMM_SET_CURRENT_REL, + COMM_CAN_FWD_FRAME, + COMM_SET_BATTERY_CUT, + COMM_SET_BLE_NAME, + COMM_SET_BLE_PIN, + COMM_SET_CAN_MODE, + COMM_GET_IMU_CALIBRATION +} COMM_PACKET_ID; + +// CAN commands +typedef enum { + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT, + CAN_PACKET_SET_CURRENT_BRAKE, + CAN_PACKET_SET_RPM, + CAN_PACKET_SET_POS, + CAN_PACKET_FILL_RX_BUFFER, + CAN_PACKET_FILL_RX_BUFFER_LONG, + CAN_PACKET_PROCESS_RX_BUFFER, + CAN_PACKET_PROCESS_SHORT_BUFFER, + CAN_PACKET_STATUS, + CAN_PACKET_SET_CURRENT_REL, + CAN_PACKET_SET_CURRENT_BRAKE_REL, + CAN_PACKET_SET_CURRENT_HANDBRAKE, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, + CAN_PACKET_STATUS_2, + CAN_PACKET_STATUS_3, + CAN_PACKET_STATUS_4, + CAN_PACKET_PING, + CAN_PACKET_PONG, + CAN_PACKET_DETECT_APPLY_ALL_FOC, + CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, + CAN_PACKET_CONF_CURRENT_LIMITS, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS, + CAN_PACKET_CONF_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_FOC_ERPMS, + CAN_PACKET_CONF_STORE_FOC_ERPMS, + CAN_PACKET_STATUS_5, + CAN_PACKET_POLL_TS5700N8501_STATUS, + CAN_PACKET_CONF_BATTERY_CUT, + CAN_PACKET_CONF_STORE_BATTERY_CUT, + CAN_PACKET_SHUTDOWN +} CAN_PACKET_ID; + +// Logged fault data +typedef struct { + uint8_t motor; + mc_fault_code fault; + float current; + float current_filtered; + float voltage; + float gate_driver_voltage; + float duty; + float rpm; + int tacho; + int cycles_running; + int tim_val_samp; + int tim_current_samp; + int tim_top; + int comm_step; + float temperature; + int drv8301_faults; +} fault_data; + +// External LED state +typedef enum { + LED_EXT_OFF = 0, + LED_EXT_NORMAL, + LED_EXT_BRAKE, + LED_EXT_TURN_LEFT, + LED_EXT_TURN_RIGHT, + LED_EXT_BRAKE_TURN_LEFT, + LED_EXT_BRAKE_TURN_RIGHT, + LED_EXT_BATT +} LED_EXT_STATE; + +typedef struct { + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; + bool rev_has_state; + bool is_rev; +} chuck_data; + +typedef struct { + int id; + systime_t rx_time; + float rpm; + float current; + float duty; +} can_status_msg; + +typedef struct { + int id; + systime_t rx_time; + float amp_hours; + float amp_hours_charged; +} can_status_msg_2; + +typedef struct { + int id; + systime_t rx_time; + float watt_hours; + float watt_hours_charged; +} can_status_msg_3; + +typedef struct { + int id; + systime_t rx_time; + float temp_fet; + float temp_motor; + float current_in; + float pid_pos_now; +} can_status_msg_4; + +typedef struct { + int id; + systime_t rx_time; + float v_in; + int32_t tacho_value; +} can_status_msg_5; + +typedef struct { + uint8_t js_x; + uint8_t js_y; + bool bt_c; + bool bt_z; + bool bt_push; + bool rev_has_state; + bool is_rev; + float vbat; +} mote_state; + +typedef enum { + MOTE_PACKET_BATT_LEVEL = 0, + MOTE_PACKET_BUTTONS, + MOTE_PACKET_ALIVE, + MOTE_PACKET_FILL_RX_BUFFER, + MOTE_PACKET_FILL_RX_BUFFER_LONG, + MOTE_PACKET_PROCESS_RX_BUFFER, + MOTE_PACKET_PROCESS_SHORT_BUFFER, + MOTE_PACKET_PAIRING_INFO +} MOTE_PACKET; + +typedef struct { + float v_in; + float temp_mos; + float temp_mos_1; + float temp_mos_2; + float temp_mos_3; + float temp_motor; + float current_motor; + float current_in; + float id; + float iq; + float rpm; + float duty_now; + float amp_hours; + float amp_hours_charged; + float watt_hours; + float watt_hours_charged; + int tachometer; + int tachometer_abs; + float position; + mc_fault_code fault_code; + int vesc_id; + float vd; + float vq; +} mc_values; + +typedef enum { + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL +} NRF_PAIR_RES; + +// Orientation data +typedef struct { + float q0; + float q1; + float q2; + float q3; + float integralFBx; + float integralFBy; + float integralFBz; + float accMagP; + int initialUpdateDone; +} ATTITUDE_INFO; + +// Custom EEPROM variables +typedef union { + uint32_t as_u32; + int32_t as_i32; + float as_float; +} eeprom_var; + +#define EEPROM_VARS_HW 64 +#define EEPROM_VARS_CUSTOM 64 + +typedef struct { + float ah_tot; + float ah_charge_tot; + float wh_tot; + float wh_charge_tot; + float current_tot; + float current_in_tot; + uint8_t num_vescs; +} setup_values; +} +#endif /* DATATYPES_H_ */ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index a1e4dbd..fa1fc3c 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -120,6 +120,19 @@ class VescPacketFWVersion : public VescPacket int fwMajor() const; int fwMinor() const; + + std::string hwname() const; + const uint8_t* uuid() const; + bool paired() const; + uint8_t devVersion() const; + + private: + int minor_; + int major_; + std::string hwname_; + bool paired_; + uint8_t uuid_[12]; + uint8_t devVersion_; }; class VescPacketRequestFWVersion : public VescPacket @@ -135,25 +148,35 @@ class VescPacketValues : public VescPacket public: explicit VescPacketValues(std::shared_ptr raw); - double v_in() const; - double temp_mos1() const; - double temp_mos2() const; - double temp_mos3() const; - double temp_mos4() const; - double temp_mos5() const; - double temp_mos6() const; - double temp_pcb() const; - double current_motor() const; - double current_in() const; - double rpm() const; - double duty_now() const; - double amp_hours() const; - double amp_hours_charged() const; - double watt_hours() const; - double watt_hours_charged() const; - double tachometer() const; - double tachometer_abs() const; - int fault_code() const; +double temp_fet() const; +double temp_motor() const; +double avg_motor_current() const; +double avg_input_current() const; +double avg_id() const; +double avg_iq() const ; +double duty_cycle_now() const; +double rpm() const; +double duty_now() const; +double v_in() const; +double amp_hours() const; +double amp_hours_charged() const; +double watt_hours() const; +double watt_hours_charged() const; +int32_t tachometer() const; +int32_t tachometer_abs() const; +int fault_code() const; +double pid_pos_now() const; +int32_t controller_id() const; + +double temp_mos1() const; +double temp_mos2() const; +double temp_mos3() const; +double avg_vd() const; +double avg_vq() const; + +int32_t numVescs() const; +double watt_battery_left() const; + }; class VescPacketRequestValues : public VescPacket @@ -161,7 +184,6 @@ class VescPacketRequestValues : public VescPacket public: VescPacketRequestValues(); }; - /*------------------------------------------------------------------------------------------------*/ class VescPacketSetDuty : public VescPacket diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index d5dc0cf..0f2fc70 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -160,28 +160,54 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa auto state_msg = VescStateStamped(); state_msg.header.stamp = now(); + state_msg.state.voltage_input = values->v_in(); - state_msg.state.temperature_pcb = values->temp_pcb(); - state_msg.state.current_motor = values->current_motor(); - state_msg.state.current_input = values->current_in(); - state_msg.state.speed = values->rpm(); - state_msg.state.duty_cycle = values->duty_now(); + state_msg.state.current_motor = values->avg_motor_current(); + state_msg.state.current_input = values->avg_input_current(); + state_msg.state.avg_id = values->avg_id(); + state_msg.state.avg_iq = values->avg_iq(); + state_msg.state.duty_cycle = values->duty_cycle_now(); + state_msg.state.speed = values->rpm(); + state_msg.state.charge_drawn = values->amp_hours(); state_msg.state.charge_regen = values->amp_hours_charged(); state_msg.state.energy_drawn = values->watt_hours(); state_msg.state.energy_regen = values->watt_hours_charged(); state_msg.state.displacement = values->tachometer(); state_msg.state.distance_traveled = values->tachometer_abs(); - state_msg.state.fault_code = values->fault_code(); + state_msg.state.fault_code = values->fault_code(); + + state_msg.state.pid_pos_now = values->pid_pos_now(); + state_msg.state.controller_id = values->controller_id(); + + state_msg.state.ntc_temp_mos1 = values->temp_mos1(); + state_msg.state.ntc_temp_mos2 = values->temp_mos2(); + state_msg.state.ntc_temp_mos3 = values->temp_mos3(); + state_msg.state.avg_vd = values->avg_vd(); + state_msg.state.avg_vq = values->avg_vq(); state_pub_->publish(state_msg); } else if (packet->name() == "FWVersion") { - std::shared_ptr fw_version = + std::shared_ptr fw_version = std::dynamic_pointer_cast(packet); // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); + + RCLCPP_INFO( + get_logger(), + "-=%s=- hardware paired %d", + fw_version->hwname().c_str(), + fw_version->paired() + ); } + + auto& clk = *this->get_clock(); + RCLCPP_INFO_THROTTLE( + get_logger(), + clk, + 5000, + "%s packet received",packet->name().c_str()); } void VescDriver::vescErrorCallback(const std::string & error) diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 5bb8df7..7ebf0cb 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -37,6 +37,7 @@ #include "vesc_driver/datatypes.hpp" #include "vesc_driver/vesc_packet_factory.hpp" +#include namespace vesc_driver { @@ -100,16 +101,59 @@ VescPacket::VescPacket(const std::string & name, std::shared_ptr raw) VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : VescPacket("FWVersion", raw) { + + major_ = *(payload_.first + 1); + minor_ = *(payload_.first + 2); + int j=0; + + + for(int i=0;*(payload_.first+3+i)!=0x00;i++){ + j = i; + hwname_ += *(payload_.first+3+i); + } + + j++; + + + + + for(int u=0;u<12;u++){ + uuid_[u] = *(payload_.first+3+j+1+u); + + } + + + paired_ = *(payload_.first+3+j+1+12); + devVersion_ = *(payload_.first+3+j+2+12+1); + } int VescPacketFWVersion::fwMajor() const { - return *(payload_.first + 1); + return major_; } int VescPacketFWVersion::fwMinor() const { - return *(payload_.first + 2); + return minor_; +} + +std::string VescPacketFWVersion::hwname() const +{ + + return hwname_.c_str(); +} +const uint8_t* VescPacketFWVersion::uuid() const +{ + return uuid_; +} +bool VescPacketFWVersion::paired() const +{ + return paired_; +} +uint8_t VescPacketFWVersion::devVersion() const +{ + return devVersion_; } REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) @@ -129,121 +173,216 @@ VescPacketValues::VescPacketValues(std::shared_ptr raw) : VescPacket("Values", raw) { } - -double VescPacketValues::temp_mos1() const -{ +double VescPacketValues::temp_fet() const{ int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); + static_cast(*(payload_.first + 2))); return static_cast(v) / 10.0; } -double VescPacketValues::temp_mos2() const -{ +double VescPacketValues::temp_motor() const{ int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - static_cast(*(payload_.first + 4))); + static_cast(*(payload_.first + 4))); 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))); + +double VescPacketValues::avg_motor_current() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + + static_cast(*(payload_.first + 8)) + ); 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))); + +double VescPacketValues::avg_input_current() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + static_cast(*(payload_.first + 12)) + ); return static_cast(v) / 100.0; + } -double VescPacketValues::duty_now() const +double VescPacketValues::avg_id() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 13)) << 24) + + (static_cast(*(payload_.first + 14)) << 16) + + (static_cast(*(payload_.first + 15)) << 8) + + static_cast(*(payload_.first + 16)) + ); + return static_cast(v) / 100.0; + +} + +double VescPacketValues::avg_iq() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 17)) << 24) + + (static_cast(*(payload_.first + 18)) << 16) + + (static_cast(*(payload_.first + 19)) << 8) + + static_cast(*(payload_.first + 20)) + ); + return static_cast(v) / 100.0; + +} + + +double VescPacketValues::duty_cycle_now() const { int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); + static_cast(*(payload_.first + 22))); return static_cast(v) / 1000.0; } -double VescPacketValues::rpm() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26))); - return static_cast(-1 * v); +double VescPacketValues::rpm() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + + static_cast(*(payload_.first + 26)) + ); + return static_cast(v) / 1.0; + } -double VescPacketValues::amp_hours() const +double VescPacketValues::v_in() const { - int32_t v = static_cast((static_cast(*(payload_.first + 27)) << 24) + - (static_cast(*(payload_.first + 28)) << 16) + - (static_cast(*(payload_.first + 29)) << 8) + - static_cast(*(payload_.first + 30))); - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 27)) << 8) + + static_cast(*(payload_.first + 28))); + return static_cast(v) /10.0; } -double VescPacketValues::amp_hours_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))); - return static_cast(v); +double VescPacketValues::amp_hours() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 29)) << 24) + + (static_cast(*(payload_.first + 30)) << 16) + + (static_cast(*(payload_.first + 31)) << 8) + + static_cast(*(payload_.first + 32)) + ); + return static_cast(v) / 1e4; + } -double VescPacketValues::tachometer() const +double VescPacketValues::amp_hours_charged() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 33)) << 24) + + (static_cast(*(payload_.first + 34)) << 16) + + (static_cast(*(payload_.first + 35)) << 8) + + static_cast(*(payload_.first + 36)) + ); + return static_cast(v) / 1e4; + +} + +double VescPacketValues::watt_hours() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 37)) << 24) + + (static_cast(*(payload_.first + 38)) << 16) + + (static_cast(*(payload_.first + 39)) << 8) + + static_cast(*(payload_.first + 40)) + ); + return static_cast(v) / 1e4; + +} + +double VescPacketValues::watt_hours_charged() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 41)) << 24) + + (static_cast(*(payload_.first + 42)) << 16) + + (static_cast(*(payload_.first + 43)) << 8) + + static_cast(*(payload_.first + 44)) + ); + return static_cast(v) / 1e4; + +} + +int32_t 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))); - return static_cast(v); + int32_t v = static_cast((static_cast(*(payload_.first + 45)) << 24) + + (static_cast(*(payload_.first + 46)) << 16) + + (static_cast(*(payload_.first + 47)) << 8) + + static_cast(*(payload_.first + 48))); + return static_cast(v); } -double VescPacketValues::tachometer_abs() const +int32_t 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))); - return static_cast(v); + int32_t v = static_cast((static_cast(*(payload_.first + 49)) << 24) + + (static_cast(*(payload_.first + 50)) << 16) + + (static_cast(*(payload_.first + 51)) << 8) + + static_cast(*(payload_.first + 52))); + return static_cast(v); } int VescPacketValues::fault_code() const { - return static_cast(*(payload_.first + 56)); + return static_cast(*(payload_.first + 53)); } -double VescPacketValues::v_in() const + +double VescPacketValues::pid_pos_now() const +{ + int32_t v = static_cast((static_cast(*(payload_.first + 54)) << 24) + + (static_cast(*(payload_.first + 55)) << 16) + + (static_cast(*(payload_.first + 56)) << 8) + + static_cast(*(payload_.first + 57)) + ); + return static_cast(v) / 1e6; + +} + +int32_t VescPacketValues::controller_id() const { - int32_t v = 0; - return static_cast(v); + //int32_t v = static_cast((static_cast(*(payload_.first + 58)) << 24) + + // (static_cast(*(payload_.first + 59)) << 16) + + // (static_cast(*(payload_.first + 60)) << 8) + + // static_cast(*(payload_.first + 61))); +// + int32_t v = static_cast((static_cast(*(payload_.first + 58) ))); + + return static_cast(v); } -double VescPacketValues::temp_pcb() const +double VescPacketValues::temp_mos1() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 59)) << 8) + + static_cast(*(payload_.first + 60))); + return static_cast(v) / 10.0; } -double VescPacketValues::watt_hours() const +double VescPacketValues::temp_mos2() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 61)) << 8) + + static_cast(*(payload_.first + 62))); + return static_cast(v) / 10.0; } -double VescPacketValues::watt_hours_charged() const +double VescPacketValues::temp_mos3() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 63)) << 8) + + static_cast(*(payload_.first + 64))); + return static_cast(v) / 10.0; } +double VescPacketValues::avg_vd() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 65)) << 24) + + (static_cast(*(payload_.first + 66)) << 16) + + (static_cast(*(payload_.first + 67)) << 8) + + static_cast(*(payload_.first + 68)) + ); + return static_cast(v) / 1e3; + +} + + +double VescPacketValues::avg_vq() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 69)) << 24) + + (static_cast(*(payload_.first + 70)) << 16) + + (static_cast(*(payload_.first + 71)) << 8) + + static_cast(*(payload_.first + 72)) + ); + return static_cast(v) / 1e3; + +} + + REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) VescPacketRequestValues::VescPacketRequestValues() diff --git a/vesc_msgs/msg/VescState.msg b/vesc_msgs/msg/VescState.msg index 276d62a..2edfd30 100644 --- a/vesc_msgs/msg/VescState.msg +++ b/vesc_msgs/msg/VescState.msg @@ -9,16 +9,33 @@ int32 FAULT_CODE_ABS_OVER_CURRENT=4 int32 FAULT_CODE_OVER_TEMP_FET=5 int32 FAULT_CODE_OVER_TEMP_MOTOR=6 +# follow the bledc firwmare: commands.c +float64 temp_fet # fet temperature +float64 temp_motor # motor temperature +float64 current_motor # motor current (ampere) avg_motor_current +float64 current_input # input current (ampere) avg_input_current +float64 avg_id +float64 avg_iq +float64 duty_cycle # duty cycle (0 to 1) duty_cycle_now +float64 speed # motor electrical speed (revolutions per minute) rpm + float64 voltage_input # input voltage (volt) -float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) -float64 current_motor # motor current (ampere) -float64 current_input # input current (ampere) -float64 speed # motor electrical speed (revolutions per minute) -float64 duty_cycle # duty cycle (0 to 1) -float64 charge_drawn # electric charge drawn from input (ampere-hour) -float64 charge_regen # electric charge regenerated to input (ampere-hour) +float64 charge_drawn # electric charge drawn from input (ampere-hours) +float64 charge_regen # electric charge regenerated to input (ampere-hour) amp_hours_charged float64 energy_drawn # energy drawn from input (watt-hour) -float64 energy_regen # energy regenerated to input (watt-hour) -float64 displacement # net tachometer (counts) -float64 distance_traveled # total tachnometer (counts) +float64 energy_regen # energy regenerated to input (watt_hours_charged) +int32 displacement # net tachometer (counts) tachometer +int32 distance_traveled # total tachnometer (counts) tachometer_abs int32 fault_code + +float64 pid_pos_now +int32 controller_id + + +float64 ntc_temp_mos1 +float64 ntc_temp_mos2 +float64 ntc_temp_mos3 +float64 avg_vd +float64 avg_vq + +#float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) From d9a8c9be43080c648e2e3f2b89f2340f4b26d1fd Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Mon, 24 May 2021 00:37:10 -0700 Subject: [PATCH 26/72] fix test results --- vesc_driver/src/vesc_driver.cpp | 8 ++++-- vesc_driver/src/vesc_interface.cpp | 44 +++++++++++++++--------------- 2 files changed, 27 insertions(+), 25 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index d5dc0cf..96ef1a1 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,9 +65,11 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - // default to /tmp/ttyV0 for debug purpose - // use socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' - // to sniff comunication data to and from vesc + /* default to /tmp/ttyV0 for debug purpose + * use : + * socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' + * to sniff comunication data to and from vesc + */ std::string port = declare_parameter("port", "/tmp/ttyV0"); // attempt to connect to the serial port diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index b56cd5d..21fec38 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -67,18 +67,16 @@ class VescInterface::Impl void VescInterface::Impl::rxThread() { - //the size varies dynamically and start from 0. capacity is 4096 fixed. + // the size varies dynamically and start from 0. capacity is 4096 fixed. Buffer buffer; buffer.reserve(4096); - //buffer with fixed size used to read from serial + // buffer with fixed size used to read from serial Buffer bufferRx(4096); while (rx_thread_run_) { int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - //std::cout << "loop " << bytes_needed < lock(serial_mutex_); + // std::lock_guard lock(serial_mutex_); boost::system::error_code ec; const size_t bytes_read = boost::asio::read( @@ -145,19 +143,21 @@ void VescInterface::Impl::rxThread() boost::asio::buffer(bufferRx, bufferRx.size()), boost::asio::transfer_exactly(bytes_to_read), ec - ); - - if (ec.value() > 0 ){ - std::ostringstream ss; - ss << "Serial port comunication error "<< ec << ec < 0) { + std::ostringstream ss; + + ss << "Serial port comunication error " << ec << ec << std::endl; + ss << "failed " << ec.failed() << std::endl; + ss << ec.value() << std::endl; + ss << ec.category().name() << std::endl; + ss << "try to read the bytes received " << std::endl; + + error_handler_(ss.str()); + } + + std::copy(bufferRx.begin(), bufferRx.begin() + bytes_read, std::back_inserter(buffer)); if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); @@ -223,7 +223,7 @@ void VescInterface::connect(const std::string & port) boost::asio::serial_port::stop_bits::one)); } catch (const std::exception & e) { std::stringstream ss; - ss << "Failed to open the serial port "<< port <<" to the VESC. " << e.what(); + ss << "Failed to open the serial port " << port << " to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } @@ -243,21 +243,21 @@ void VescInterface::disconnect() impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); - //std::lock_guard lock(impl_->serial_mutex_); + // std::lock_guard lock(impl_->serial_mutex_); impl_->serial_port_.close(); } } bool VescInterface::isConnected() const { - //std::lock_guard lock(impl_->serial_mutex_); + // std::lock_guard lock(impl_->serial_mutex_); return impl_->serial_port_.is_open(); } void VescInterface::send(const VescPacket & packet) { try { - //std::lock_guard lock(impl_->serial_mutex_); + // std::lock_guard lock(impl_->serial_mutex_); size_t written = impl_->serial_port_.write_some( boost::asio::buffer(packet.frame())); if (written != packet.frame().size()) { From 01f6304eda6d0375f666571ffd72173549824c49 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Mon, 24 May 2021 02:33:45 -0700 Subject: [PATCH 27/72] fixed test results --- vesc_driver/include/vesc_driver/datatypes.hpp | 1700 +++++++++-------- .../include/vesc_driver/vesc_packet.hpp | 67 +- vesc_driver/src/vesc_driver.cpp | 54 +- vesc_driver/src/vesc_packet.cpp | 303 +-- 4 files changed, 1101 insertions(+), 1023 deletions(-) diff --git a/vesc_driver/include/vesc_driver/datatypes.hpp b/vesc_driver/include/vesc_driver/datatypes.hpp index c3043e4..8548f76 100644 --- a/vesc_driver/include/vesc_driver/datatypes.hpp +++ b/vesc_driver/include/vesc_driver/datatypes.hpp @@ -28,26 +28,26 @@ /* - Some parts of this code, Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se + Some parts of this code, Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se - This file is part of the VESC firmware. + This file is part of the VESC firmware. - The VESC firmware is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. - The VESC firmware is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ -#ifndef DATATYPES_H_ -#define DATATYPES_H_ +#ifndef VESC_DRIVER__DATATYPES_HPP_ +#define VESC_DRIVER__DATATYPES_HPP_ #include #include @@ -88,959 +88,1019 @@ typedef enum VESC_TX_DOUBLE32_AUTO } VESC_TX_T; -//#include "ch.h" -typedef uint32_t systime_t; // defined in ch.h +// #include "ch.h" +typedef uint32_t systime_t; // defined in ch.h // Data types -typedef enum { - MC_STATE_OFF = 0, - MC_STATE_DETECTING, - MC_STATE_RUNNING, - MC_STATE_FULL_BRAKE, +typedef enum +{ + MC_STATE_OFF = 0, + MC_STATE_DETECTING, + MC_STATE_RUNNING, + MC_STATE_FULL_BRAKE, } mc_state; -typedef enum { - PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended - PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode - PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs +typedef enum +{ + PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended + PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode + PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs } mc_pwm_mode; -typedef enum { - COMM_MODE_INTEGRATE = 0, - COMM_MODE_DELAY +typedef enum +{ + COMM_MODE_INTEGRATE = 0, + COMM_MODE_DELAY } mc_comm_mode; -typedef enum { - SENSOR_MODE_SENSORLESS = 0, - SENSOR_MODE_SENSORED, - SENSOR_MODE_HYBRID +typedef enum +{ + SENSOR_MODE_SENSORLESS = 0, + SENSOR_MODE_SENSORED, + SENSOR_MODE_HYBRID } mc_sensor_mode; -typedef enum { - FOC_SENSOR_MODE_SENSORLESS = 0, - FOC_SENSOR_MODE_ENCODER, - FOC_SENSOR_MODE_HALL, - FOC_SENSOR_MODE_HFI +typedef enum +{ + FOC_SENSOR_MODE_SENSORLESS = 0, + FOC_SENSOR_MODE_ENCODER, + FOC_SENSOR_MODE_HALL, + FOC_SENSOR_MODE_HFI } mc_foc_sensor_mode; // Auxiliary output mode -typedef enum { - OUT_AUX_MODE_OFF = 0, - OUT_AUX_MODE_ON_AFTER_2S, - OUT_AUX_MODE_ON_AFTER_5S, - OUT_AUX_MODE_ON_AFTER_10S, - OUT_AUX_MODE_UNUSED +typedef enum +{ + OUT_AUX_MODE_OFF = 0, + OUT_AUX_MODE_ON_AFTER_2S, + OUT_AUX_MODE_ON_AFTER_5S, + OUT_AUX_MODE_ON_AFTER_10S, + OUT_AUX_MODE_UNUSED } out_aux_mode; // Temperature sensor type -typedef enum { - TEMP_SENSOR_NTC_10K_25C = 0, - TEMP_SENSOR_PTC_1K_100C, - TEMP_SENSOR_KTY83_122, +typedef enum +{ + TEMP_SENSOR_NTC_10K_25C = 0, + TEMP_SENSOR_PTC_1K_100C, + TEMP_SENSOR_KTY83_122, } temp_sensor_type; // General purpose drive output mode -typedef enum { - GPD_OUTPUT_MODE_NONE = 0, - GPD_OUTPUT_MODE_MODULATION, - GPD_OUTPUT_MODE_VOLTAGE, - GPD_OUTPUT_MODE_CURRENT +typedef enum +{ + GPD_OUTPUT_MODE_NONE = 0, + GPD_OUTPUT_MODE_MODULATION, + GPD_OUTPUT_MODE_VOLTAGE, + GPD_OUTPUT_MODE_CURRENT } gpd_output_mode; -typedef enum { - MOTOR_TYPE_BLDC = 0, - MOTOR_TYPE_DC, - MOTOR_TYPE_FOC, - MOTOR_TYPE_GPD +typedef enum +{ + MOTOR_TYPE_BLDC = 0, + MOTOR_TYPE_DC, + MOTOR_TYPE_FOC, + MOTOR_TYPE_GPD } mc_motor_type; // FOC current controller decoupling mode. -typedef enum { - FOC_CC_DECOUPLING_DISABLED = 0, - FOC_CC_DECOUPLING_CROSS, - FOC_CC_DECOUPLING_BEMF, - FOC_CC_DECOUPLING_CROSS_BEMF +typedef enum +{ + FOC_CC_DECOUPLING_DISABLED = 0, + FOC_CC_DECOUPLING_CROSS, + FOC_CC_DECOUPLING_BEMF, + FOC_CC_DECOUPLING_CROSS_BEMF } mc_foc_cc_decoupling_mode; -typedef enum { - FOC_OBSERVER_ORTEGA_ORIGINAL = 0, - FOC_OBSERVER_ORTEGA_ITERATIVE +typedef enum +{ + FOC_OBSERVER_ORTEGA_ORIGINAL = 0, + FOC_OBSERVER_ORTEGA_ITERATIVE } mc_foc_observer_type; -typedef enum { - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR, - FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, - FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, - FAULT_CODE_MCU_UNDER_VOLTAGE, - FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, - FAULT_CODE_ENCODER_SPI, - FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, - FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, - FAULT_CODE_FLASH_CORRUPTION, - FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, - FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, - FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, - FAULT_CODE_UNBALANCED_CURRENTS, - FAULT_CODE_BRK, - FAULT_CODE_RESOLVER_LOT, - FAULT_CODE_RESOLVER_DOS, - FAULT_CODE_RESOLVER_LOS +typedef enum +{ + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR, + FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, + FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, + FAULT_CODE_MCU_UNDER_VOLTAGE, + FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, + FAULT_CODE_ENCODER_SPI, + FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, + FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, + FAULT_CODE_FLASH_CORRUPTION, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, + FAULT_CODE_UNBALANCED_CURRENTS, + FAULT_CODE_BRK, + FAULT_CODE_RESOLVER_LOT, + FAULT_CODE_RESOLVER_DOS, + FAULT_CODE_RESOLVER_LOS } mc_fault_code; -typedef enum { - CONTROL_MODE_DUTY = 0, - CONTROL_MODE_SPEED, - CONTROL_MODE_CURRENT, - CONTROL_MODE_CURRENT_BRAKE, - CONTROL_MODE_POS, - CONTROL_MODE_HANDBRAKE, - CONTROL_MODE_OPENLOOP, - CONTROL_MODE_OPENLOOP_PHASE, - CONTROL_MODE_OPENLOOP_DUTY, - CONTROL_MODE_OPENLOOP_DUTY_PHASE, - CONTROL_MODE_NONE +typedef enum +{ + CONTROL_MODE_DUTY = 0, + CONTROL_MODE_SPEED, + CONTROL_MODE_CURRENT, + CONTROL_MODE_CURRENT_BRAKE, + CONTROL_MODE_POS, + CONTROL_MODE_HANDBRAKE, + CONTROL_MODE_OPENLOOP, + CONTROL_MODE_OPENLOOP_PHASE, + CONTROL_MODE_OPENLOOP_DUTY, + CONTROL_MODE_OPENLOOP_DUTY_PHASE, + CONTROL_MODE_NONE } mc_control_mode; -typedef enum { - DISP_POS_MODE_NONE = 0, - DISP_POS_MODE_INDUCTANCE, - DISP_POS_MODE_OBSERVER, - DISP_POS_MODE_ENCODER, - DISP_POS_MODE_PID_POS, - DISP_POS_MODE_PID_POS_ERROR, - DISP_POS_MODE_ENCODER_OBSERVER_ERROR +typedef enum +{ + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR } disp_pos_mode; -typedef enum { - SENSOR_PORT_MODE_HALL = 0, - SENSOR_PORT_MODE_ABI, - SENSOR_PORT_MODE_AS5047_SPI, - SENSOR_PORT_MODE_AD2S1205, - SENSOR_PORT_MODE_SINCOS, - SENSOR_PORT_MODE_TS5700N8501, - SENSOR_PORT_MODE_TS5700N8501_MULTITURN +typedef enum +{ + SENSOR_PORT_MODE_HALL = 0, + SENSOR_PORT_MODE_ABI, + SENSOR_PORT_MODE_AS5047_SPI, + SENSOR_PORT_MODE_AD2S1205, + SENSOR_PORT_MODE_SINCOS, + SENSOR_PORT_MODE_TS5700N8501, + SENSOR_PORT_MODE_TS5700N8501_MULTITURN } sensor_port_mode; -typedef struct { - float cycle_int_limit; - float cycle_int_limit_running; - float cycle_int_limit_max; - float comm_time_sum; - float comm_time_sum_min_rpm; - int32_t comms; - float time_at_comm; +typedef struct +{ + float cycle_int_limit; + float cycle_int_limit_running; + float cycle_int_limit_max; + float comm_time_sum; + float comm_time_sum_min_rpm; + int32_t comms; + float time_at_comm; } mc_rpm_dep_struct; -typedef enum { - DRV8301_OC_LIMIT = 0, - DRV8301_OC_LATCH_SHUTDOWN, - DRV8301_OC_REPORT_ONLY, - DRV8301_OC_DISABLED +typedef enum +{ + DRV8301_OC_LIMIT = 0, + DRV8301_OC_LATCH_SHUTDOWN, + DRV8301_OC_REPORT_ONLY, + DRV8301_OC_DISABLED } drv8301_oc_mode; -typedef enum { - DEBUG_SAMPLING_OFF = 0, - DEBUG_SAMPLING_NOW, - DEBUG_SAMPLING_START, - DEBUG_SAMPLING_TRIGGER_START, - DEBUG_SAMPLING_TRIGGER_FAULT, - DEBUG_SAMPLING_TRIGGER_START_NOSEND, - DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, - DEBUG_SAMPLING_SEND_LAST_SAMPLES +typedef enum +{ + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES } debug_sampling_mode; -typedef enum { - CAN_BAUD_125K = 0, - CAN_BAUD_250K, - CAN_BAUD_500K, - CAN_BAUD_1M, - CAN_BAUD_10K, - CAN_BAUD_20K, - CAN_BAUD_50K, - CAN_BAUD_75K +typedef enum +{ + CAN_BAUD_125K = 0, + CAN_BAUD_250K, + CAN_BAUD_500K, + CAN_BAUD_1M, + CAN_BAUD_10K, + CAN_BAUD_20K, + CAN_BAUD_50K, + CAN_BAUD_75K } CAN_BAUD; -typedef enum { - BATTERY_TYPE_LIION_3_0__4_2, - BATTERY_TYPE_LIIRON_2_6__3_6, - BATTERY_TYPE_LEAD_ACID +typedef enum +{ + BATTERY_TYPE_LIION_3_0__4_2, + BATTERY_TYPE_LIIRON_2_6__3_6, + BATTERY_TYPE_LEAD_ACID } BATTERY_TYPE; -typedef enum { - HFI_SAMPLES_8 = 0, - HFI_SAMPLES_16, - HFI_SAMPLES_32 +typedef enum +{ + HFI_SAMPLES_8 = 0, + HFI_SAMPLES_16, + HFI_SAMPLES_32 } FOC_HFI_SAMPLES; -typedef struct { - // Switching and drive - mc_pwm_mode pwm_mode; - mc_comm_mode comm_mode; - mc_motor_type motor_type; - mc_sensor_mode sensor_mode; - // Limits - float l_current_max; - float l_current_min; - float l_in_current_max; - float l_in_current_min; - float l_abs_current_max; - float l_min_erpm; - float l_max_erpm; - float l_erpm_start; - float l_max_erpm_fbrake; - float l_max_erpm_fbrake_cc; - float l_min_vin; - float l_max_vin; - float l_battery_cut_start; - float l_battery_cut_end; - bool l_slow_abs_current; - float l_temp_fet_start; - float l_temp_fet_end; - float l_temp_motor_start; - float l_temp_motor_end; - float l_temp_accel_dec; - float l_min_duty; - float l_max_duty; - float l_watt_max; - float l_watt_min; - float l_current_max_scale; - float l_current_min_scale; - float l_duty_start; - // Overridden limits (Computed during runtime) - float lo_current_max; - float lo_current_min; - float lo_in_current_max; - float lo_in_current_min; - float lo_current_motor_max_now; - float lo_current_motor_min_now; - // Sensorless (bldc) - float sl_min_erpm; - float sl_min_erpm_cycle_int_limit; - float sl_max_fullbreak_current_dir_change; - float sl_cycle_int_limit; - float sl_phase_advance_at_br; - float sl_cycle_int_rpm_br; - float sl_bemf_coupling_k; - // Hall sensor - int8_t hall_table[8]; - float hall_sl_erpm; - // FOC - float foc_current_kp; - float foc_current_ki; - float foc_f_sw; - float foc_dt_us; - float foc_encoder_offset; - bool foc_encoder_inverted; - float foc_encoder_ratio; - float foc_encoder_sin_offset; - float foc_encoder_sin_gain; - float foc_encoder_cos_offset; - float foc_encoder_cos_gain; - float foc_encoder_sincos_filter_constant; - float foc_motor_l; - float foc_motor_r; - float foc_motor_flux_linkage; - float foc_observer_gain; - float foc_observer_gain_slow; - float foc_pll_kp; - float foc_pll_ki; - float foc_duty_dowmramp_kp; - float foc_duty_dowmramp_ki; - float foc_openloop_rpm; - float foc_sl_openloop_hyst; - float foc_sl_openloop_time; - float foc_sl_d_current_duty; - float foc_sl_d_current_factor; - mc_foc_sensor_mode foc_sensor_mode; - uint8_t foc_hall_table[8]; - float foc_sl_erpm; - bool foc_sample_v0_v7; - bool foc_sample_high_current; - float foc_sat_comp; - bool foc_temp_comp; - float foc_temp_comp_base_temp; - float foc_current_filter_const; - mc_foc_cc_decoupling_mode foc_cc_decoupling; - mc_foc_observer_type foc_observer_type; - float foc_hfi_voltage_start; - float foc_hfi_voltage_run; - float foc_hfi_voltage_max; - float foc_sl_erpm_hfi; - uint16_t foc_hfi_start_samples; - float foc_hfi_obs_ovr_sec; - FOC_HFI_SAMPLES foc_hfi_samples; - // GPDrive - int gpd_buffer_notify_left; - int gpd_buffer_interpol; - float gpd_current_filter_const; - float gpd_current_kp; - float gpd_current_ki; - // Speed PID - float s_pid_kp; - float s_pid_ki; - float s_pid_kd; - float s_pid_kd_filter; - float s_pid_min_erpm; - bool s_pid_allow_braking; - // Pos PID - float p_pid_kp; - float p_pid_ki; - float p_pid_kd; - float p_pid_kd_filter; - float p_pid_ang_div; - // Current controller - float cc_startup_boost_duty; - float cc_min_current; - float cc_gain; - float cc_ramp_step_max; - // Misc - int32_t m_fault_stop_time_ms; - float m_duty_ramp_step; - float m_current_backoff_gain; - uint32_t m_encoder_counts; - sensor_port_mode m_sensor_port_mode; - bool m_invert_direction; - drv8301_oc_mode m_drv8301_oc_mode; - int m_drv8301_oc_adj; - float m_bldc_f_sw_min; - float m_bldc_f_sw_max; - float m_dc_f_sw; - float m_ntc_motor_beta; - out_aux_mode m_out_aux_mode; - temp_sensor_type m_motor_temp_sens_type; - float m_ptc_motor_coeff; - // Setup info - uint8_t si_motor_poles; - float si_gear_ratio; - float si_wheel_diameter; - BATTERY_TYPE si_battery_type; - int si_battery_cells; - float si_battery_ah; +typedef struct +{ + // Switching and drive + mc_pwm_mode pwm_mode; + mc_comm_mode comm_mode; + mc_motor_type motor_type; + mc_sensor_mode sensor_mode; + // Limits + float l_current_max; + float l_current_min; + float l_in_current_max; + float l_in_current_min; + float l_abs_current_max; + float l_min_erpm; + float l_max_erpm; + float l_erpm_start; + float l_max_erpm_fbrake; + float l_max_erpm_fbrake_cc; + float l_min_vin; + float l_max_vin; + float l_battery_cut_start; + float l_battery_cut_end; + bool l_slow_abs_current; + float l_temp_fet_start; + float l_temp_fet_end; + float l_temp_motor_start; + float l_temp_motor_end; + float l_temp_accel_dec; + float l_min_duty; + float l_max_duty; + float l_watt_max; + float l_watt_min; + float l_current_max_scale; + float l_current_min_scale; + float l_duty_start; + // Overridden limits (Computed during runtime) + float lo_current_max; + float lo_current_min; + float lo_in_current_max; + float lo_in_current_min; + float lo_current_motor_max_now; + float lo_current_motor_min_now; + // Sensorless (bldc) + float sl_min_erpm; + float sl_min_erpm_cycle_int_limit; + float sl_max_fullbreak_current_dir_change; + float sl_cycle_int_limit; + float sl_phase_advance_at_br; + float sl_cycle_int_rpm_br; + float sl_bemf_coupling_k; + // Hall sensor + int8_t hall_table[8]; + float hall_sl_erpm; + // FOC + float foc_current_kp; + float foc_current_ki; + float foc_f_sw; + float foc_dt_us; + float foc_encoder_offset; + bool foc_encoder_inverted; + float foc_encoder_ratio; + float foc_encoder_sin_offset; + float foc_encoder_sin_gain; + float foc_encoder_cos_offset; + float foc_encoder_cos_gain; + float foc_encoder_sincos_filter_constant; + float foc_motor_l; + float foc_motor_r; + float foc_motor_flux_linkage; + float foc_observer_gain; + float foc_observer_gain_slow; + float foc_pll_kp; + float foc_pll_ki; + float foc_duty_dowmramp_kp; + float foc_duty_dowmramp_ki; + float foc_openloop_rpm; + float foc_sl_openloop_hyst; + float foc_sl_openloop_time; + float foc_sl_d_current_duty; + float foc_sl_d_current_factor; + mc_foc_sensor_mode foc_sensor_mode; + uint8_t foc_hall_table[8]; + float foc_sl_erpm; + bool foc_sample_v0_v7; + bool foc_sample_high_current; + float foc_sat_comp; + bool foc_temp_comp; + float foc_temp_comp_base_temp; + float foc_current_filter_const; + mc_foc_cc_decoupling_mode foc_cc_decoupling; + mc_foc_observer_type foc_observer_type; + float foc_hfi_voltage_start; + float foc_hfi_voltage_run; + float foc_hfi_voltage_max; + float foc_sl_erpm_hfi; + uint16_t foc_hfi_start_samples; + float foc_hfi_obs_ovr_sec; + FOC_HFI_SAMPLES foc_hfi_samples; + // GPDrive + int gpd_buffer_notify_left; + int gpd_buffer_interpol; + float gpd_current_filter_const; + float gpd_current_kp; + float gpd_current_ki; + // Speed PID + float s_pid_kp; + float s_pid_ki; + float s_pid_kd; + float s_pid_kd_filter; + float s_pid_min_erpm; + bool s_pid_allow_braking; + // Pos PID + float p_pid_kp; + float p_pid_ki; + float p_pid_kd; + float p_pid_kd_filter; + float p_pid_ang_div; + // Current controller + float cc_startup_boost_duty; + float cc_min_current; + float cc_gain; + float cc_ramp_step_max; + // Misc + int32_t m_fault_stop_time_ms; + float m_duty_ramp_step; + float m_current_backoff_gain; + uint32_t m_encoder_counts; + sensor_port_mode m_sensor_port_mode; + bool m_invert_direction; + drv8301_oc_mode m_drv8301_oc_mode; + int m_drv8301_oc_adj; + float m_bldc_f_sw_min; + float m_bldc_f_sw_max; + float m_dc_f_sw; + float m_ntc_motor_beta; + out_aux_mode m_out_aux_mode; + temp_sensor_type m_motor_temp_sens_type; + float m_ptc_motor_coeff; + // Setup info + uint8_t si_motor_poles; + float si_gear_ratio; + float si_wheel_diameter; + BATTERY_TYPE si_battery_type; + int si_battery_cells; + float si_battery_ah; } mc_configuration; // Applications to use -typedef enum { - APP_NONE = 0, - APP_PPM, - APP_ADC, - APP_UART, - APP_PPM_UART, - APP_ADC_UART, - APP_NUNCHUK, - APP_NRF, - APP_CUSTOM, - APP_BALANCE +typedef enum +{ + APP_NONE = 0, + APP_PPM, + APP_ADC, + APP_UART, + APP_PPM_UART, + APP_ADC_UART, + APP_NUNCHUK, + APP_NRF, + APP_CUSTOM, + APP_BALANCE } app_use; // Throttle curve mode -typedef enum { - THR_EXP_EXPO = 0, - THR_EXP_NATURAL, - THR_EXP_POLY +typedef enum +{ + THR_EXP_EXPO = 0, + THR_EXP_NATURAL, + THR_EXP_POLY } thr_exp_mode; // PPM control types -typedef enum { - PPM_CTRL_TYPE_NONE = 0, - PPM_CTRL_TYPE_CURRENT, - PPM_CTRL_TYPE_CURRENT_NOREV, - PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, - PPM_CTRL_TYPE_DUTY, - PPM_CTRL_TYPE_DUTY_NOREV, - PPM_CTRL_TYPE_PID, - PPM_CTRL_TYPE_PID_NOREV, - PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST, - PPM_CTRL_TYPE_CURRENT_SMART_REV +typedef enum +{ + PPM_CTRL_TYPE_NONE = 0, + PPM_CTRL_TYPE_CURRENT, + PPM_CTRL_TYPE_CURRENT_NOREV, + PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, + PPM_CTRL_TYPE_DUTY, + PPM_CTRL_TYPE_DUTY_NOREV, + PPM_CTRL_TYPE_PID, + PPM_CTRL_TYPE_PID_NOREV, + PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST, + PPM_CTRL_TYPE_CURRENT_SMART_REV } ppm_control_type; -typedef struct { - ppm_control_type ctrl_type; - float pid_max_erpm; - float hyst; - float pulse_start; - float pulse_end; - float pulse_center; - bool median_filter; - bool safe_start; - float throttle_exp; - float throttle_exp_brake; - thr_exp_mode throttle_exp_mode; - float ramp_time_pos; - float ramp_time_neg; - bool multi_esc; - bool tc; - float tc_max_diff; - float max_erpm_for_dir; - float smart_rev_max_duty; - float smart_rev_ramp_time; +typedef struct +{ + ppm_control_type ctrl_type; + float pid_max_erpm; + float hyst; + float pulse_start; + float pulse_end; + float pulse_center; + bool median_filter; + bool safe_start; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + float max_erpm_for_dir; + float smart_rev_max_duty; + float smart_rev_ramp_time; } ppm_config; // ADC control types -typedef enum { - ADC_CTRL_TYPE_NONE = 0, - ADC_CTRL_TYPE_CURRENT, - ADC_CTRL_TYPE_CURRENT_REV_CENTER, - ADC_CTRL_TYPE_CURRENT_REV_BUTTON, - ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, - ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, - ADC_CTRL_TYPE_DUTY, - ADC_CTRL_TYPE_DUTY_REV_CENTER, - ADC_CTRL_TYPE_DUTY_REV_BUTTON, - ADC_CTRL_TYPE_PID, - ADC_CTRL_TYPE_PID_REV_CENTER, - ADC_CTRL_TYPE_PID_REV_BUTTON +typedef enum +{ + ADC_CTRL_TYPE_NONE = 0, + ADC_CTRL_TYPE_CURRENT, + ADC_CTRL_TYPE_CURRENT_REV_CENTER, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, + ADC_CTRL_TYPE_DUTY, + ADC_CTRL_TYPE_DUTY_REV_CENTER, + ADC_CTRL_TYPE_DUTY_REV_BUTTON, + ADC_CTRL_TYPE_PID, + ADC_CTRL_TYPE_PID_REV_CENTER, + ADC_CTRL_TYPE_PID_REV_BUTTON } adc_control_type; -typedef struct { - adc_control_type ctrl_type; - float hyst; - float voltage_start; - float voltage_end; - float voltage_center; - float voltage2_start; - float voltage2_end; - bool use_filter; - bool safe_start; - bool cc_button_inverted; - bool rev_button_inverted; - bool voltage_inverted; - bool voltage2_inverted; - float throttle_exp; - float throttle_exp_brake; - thr_exp_mode throttle_exp_mode; - float ramp_time_pos; - float ramp_time_neg; - bool multi_esc; - bool tc; - float tc_max_diff; - uint32_t update_rate_hz; +typedef struct +{ + adc_control_type ctrl_type; + float hyst; + float voltage_start; + float voltage_end; + float voltage_center; + float voltage2_start; + float voltage2_end; + bool use_filter; + bool safe_start; + bool cc_button_inverted; + bool rev_button_inverted; + bool voltage_inverted; + bool voltage2_inverted; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + uint32_t update_rate_hz; } adc_config; // Nunchuk control types -typedef enum { - CHUK_CTRL_TYPE_NONE = 0, - CHUK_CTRL_TYPE_CURRENT, - CHUK_CTRL_TYPE_CURRENT_NOREV +typedef enum +{ + CHUK_CTRL_TYPE_NONE = 0, + CHUK_CTRL_TYPE_CURRENT, + CHUK_CTRL_TYPE_CURRENT_NOREV } chuk_control_type; -typedef struct { - chuk_control_type ctrl_type; - float hyst; - float ramp_time_pos; - float ramp_time_neg; - float stick_erpm_per_s_in_cc; - float throttle_exp; - float throttle_exp_brake; - thr_exp_mode throttle_exp_mode; - bool multi_esc; - bool tc; - float tc_max_diff; - bool use_smart_rev; - float smart_rev_max_duty; - float smart_rev_ramp_time; +typedef struct +{ + chuk_control_type ctrl_type; + float hyst; + float ramp_time_pos; + float ramp_time_neg; + float stick_erpm_per_s_in_cc; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + bool multi_esc; + bool tc; + float tc_max_diff; + bool use_smart_rev; + float smart_rev_max_duty; + float smart_rev_ramp_time; } chuk_config; // NRF Datatypes -typedef enum { - NRF_SPEED_250K = 0, - NRF_SPEED_1M, - NRF_SPEED_2M +typedef enum +{ + NRF_SPEED_250K = 0, + NRF_SPEED_1M, + NRF_SPEED_2M } NRF_SPEED; -typedef enum { - NRF_POWER_M18DBM = 0, - NRF_POWER_M12DBM, - NRF_POWER_M6DBM, - NRF_POWER_0DBM, +typedef enum +{ + NRF_POWER_M18DBM = 0, + NRF_POWER_M12DBM, + NRF_POWER_M6DBM, + NRF_POWER_0DBM, NRF_POWER_OFF } NRF_POWER; -typedef enum { - NRF_AW_3 = 0, - NRF_AW_4, - NRF_AW_5 +typedef enum +{ + NRF_AW_3 = 0, + NRF_AW_4, + NRF_AW_5 } NRF_AW; -typedef enum { - NRF_CRC_DISABLED = 0, - NRF_CRC_1B, - NRF_CRC_2B +typedef enum +{ + NRF_CRC_DISABLED = 0, + NRF_CRC_1B, + NRF_CRC_2B } NRF_CRC; -typedef enum { - NRF_RETR_DELAY_250US = 0, - NRF_RETR_DELAY_500US, - NRF_RETR_DELAY_750US, - NRF_RETR_DELAY_1000US, - NRF_RETR_DELAY_1250US, - NRF_RETR_DELAY_1500US, - NRF_RETR_DELAY_1750US, - NRF_RETR_DELAY_2000US, - NRF_RETR_DELAY_2250US, - NRF_RETR_DELAY_2500US, - NRF_RETR_DELAY_2750US, - NRF_RETR_DELAY_3000US, - NRF_RETR_DELAY_3250US, - NRF_RETR_DELAY_3500US, - NRF_RETR_DELAY_3750US, - NRF_RETR_DELAY_4000US +typedef enum +{ + NRF_RETR_DELAY_250US = 0, + NRF_RETR_DELAY_500US, + NRF_RETR_DELAY_750US, + NRF_RETR_DELAY_1000US, + NRF_RETR_DELAY_1250US, + NRF_RETR_DELAY_1500US, + NRF_RETR_DELAY_1750US, + NRF_RETR_DELAY_2000US, + NRF_RETR_DELAY_2250US, + NRF_RETR_DELAY_2500US, + NRF_RETR_DELAY_2750US, + NRF_RETR_DELAY_3000US, + NRF_RETR_DELAY_3250US, + NRF_RETR_DELAY_3500US, + NRF_RETR_DELAY_3750US, + NRF_RETR_DELAY_4000US } NRF_RETR_DELAY; -typedef struct { - NRF_SPEED speed; - NRF_POWER power; - NRF_CRC crc_type; - NRF_RETR_DELAY retry_delay; - unsigned char retries; - unsigned char channel; - unsigned char address[3]; - bool send_crc_ack; +typedef struct +{ + NRF_SPEED speed; + NRF_POWER power; + NRF_CRC crc_type; + NRF_RETR_DELAY retry_delay; + unsigned char retries; + unsigned char channel; + unsigned char address[3]; + bool send_crc_ack; } nrf_config; -typedef struct { - float kp; - float ki; - float kd; - uint16_t hertz; - float pitch_fault; - float roll_fault; - float adc1; - float adc2; - float overspeed_duty; - float tiltback_duty; - float tiltback_angle; - float tiltback_speed; - float tiltback_high_voltage; - float tiltback_low_voltage; - float startup_pitch_tolerance; - float startup_roll_tolerance; - float startup_speed; - float deadzone; - float current_boost; - bool multi_esc; - float yaw_kp; - float yaw_ki; - float yaw_kd; - float roll_steer_kp; - float brake_current; - uint16_t overspeed_delay; - uint16_t fault_delay; - float tiltback_constant; - float roll_steer_erpm_kp; - float yaw_current_clamp; - uint16_t adc_half_fault_erpm; - float setpoint_pitch_filter; - float setpoint_target_filter; - float setpoint_clamp; +typedef struct +{ + float kp; + float ki; + float kd; + uint16_t hertz; + float pitch_fault; + float roll_fault; + float adc1; + float adc2; + float overspeed_duty; + float tiltback_duty; + float tiltback_angle; + float tiltback_speed; + float tiltback_high_voltage; + float tiltback_low_voltage; + float startup_pitch_tolerance; + float startup_roll_tolerance; + float startup_speed; + float deadzone; + float current_boost; + bool multi_esc; + float yaw_kp; + float yaw_ki; + float yaw_kd; + float roll_steer_kp; + float brake_current; + uint16_t overspeed_delay; + uint16_t fault_delay; + float tiltback_constant; + float roll_steer_erpm_kp; + float yaw_current_clamp; + uint16_t adc_half_fault_erpm; + float setpoint_pitch_filter; + float setpoint_target_filter; + float setpoint_clamp; } balance_config; // CAN status modes -typedef enum { - CAN_STATUS_DISABLED = 0, - CAN_STATUS_1, - CAN_STATUS_1_2, - CAN_STATUS_1_2_3, - CAN_STATUS_1_2_3_4, - CAN_STATUS_1_2_3_4_5 +typedef enum +{ + CAN_STATUS_DISABLED = 0, + CAN_STATUS_1, + CAN_STATUS_1_2, + CAN_STATUS_1_2_3, + CAN_STATUS_1_2_3_4, + CAN_STATUS_1_2_3_4_5 } CAN_STATUS_MODE; -typedef enum { - SHUTDOWN_MODE_ALWAYS_OFF = 0, - SHUTDOWN_MODE_ALWAYS_ON, - SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY, - SHUTDOWN_MODE_OFF_AFTER_10S, - SHUTDOWN_MODE_OFF_AFTER_1M, - SHUTDOWN_MODE_OFF_AFTER_5M, - SHUTDOWN_MODE_OFF_AFTER_10M, - SHUTDOWN_MODE_OFF_AFTER_30M, - SHUTDOWN_MODE_OFF_AFTER_1H, - SHUTDOWN_MODE_OFF_AFTER_5H, +typedef enum +{ + SHUTDOWN_MODE_ALWAYS_OFF = 0, + SHUTDOWN_MODE_ALWAYS_ON, + SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY, + SHUTDOWN_MODE_OFF_AFTER_10S, + SHUTDOWN_MODE_OFF_AFTER_1M, + SHUTDOWN_MODE_OFF_AFTER_5M, + SHUTDOWN_MODE_OFF_AFTER_10M, + SHUTDOWN_MODE_OFF_AFTER_30M, + SHUTDOWN_MODE_OFF_AFTER_1H, + SHUTDOWN_MODE_OFF_AFTER_5H, } SHUTDOWN_MODE; -typedef enum { - IMU_TYPE_OFF = 0, - IMU_TYPE_INTERNAL, - IMU_TYPE_EXTERNAL_MPU9X50, - IMU_TYPE_EXTERNAL_ICM20948, - IMU_TYPE_EXTERNAL_BMI160 +typedef enum +{ + IMU_TYPE_OFF = 0, + IMU_TYPE_INTERNAL, + IMU_TYPE_EXTERNAL_MPU9X50, + IMU_TYPE_EXTERNAL_ICM20948, + IMU_TYPE_EXTERNAL_BMI160 } IMU_TYPE; -typedef enum { - AHRS_MODE_MADGWICK = 0, - AHRS_MODE_MAHONY +typedef enum +{ + AHRS_MODE_MADGWICK = 0, + AHRS_MODE_MAHONY } AHRS_MODE; -typedef struct { - IMU_TYPE type; - AHRS_MODE mode; - int sample_rate_hz; - float accel_confidence_decay; - float mahony_kp; - float mahony_ki; - float madgwick_beta; - float rot_roll; - float rot_pitch; - float rot_yaw; - float accel_offsets[3]; - float gyro_offsets[3]; - float gyro_offset_comp_fact[3]; - float gyro_offset_comp_clamp; +typedef struct +{ + IMU_TYPE type; + AHRS_MODE mode; + int sample_rate_hz; + float accel_confidence_decay; + float mahony_kp; + float mahony_ki; + float madgwick_beta; + float rot_roll; + float rot_pitch; + float rot_yaw; + float accel_offsets[3]; + float gyro_offsets[3]; + float gyro_offset_comp_fact[3]; + float gyro_offset_comp_clamp; } imu_config; -typedef enum { - CAN_MODE_VESC = 0, - CAN_MODE_UAVCAN, - CAN_MODE_COMM_BRIDGE +typedef enum +{ + CAN_MODE_VESC = 0, + CAN_MODE_UAVCAN, + CAN_MODE_COMM_BRIDGE } CAN_MODE; -typedef struct { - // Settings - uint8_t controller_id; - uint32_t timeout_msec; - float timeout_brake_current; - CAN_STATUS_MODE send_can_status; - uint32_t send_can_status_rate_hz; - CAN_BAUD can_baud_rate; - bool pairing_done; - bool permanent_uart_enabled; - SHUTDOWN_MODE shutdown_mode; +typedef struct +{ + // Settings + uint8_t controller_id; + uint32_t timeout_msec; + float timeout_brake_current; + CAN_STATUS_MODE send_can_status; + uint32_t send_can_status_rate_hz; + CAN_BAUD can_baud_rate; + bool pairing_done; + bool permanent_uart_enabled; + SHUTDOWN_MODE shutdown_mode; - // CAN modes - CAN_MODE can_mode; - uint8_t uavcan_esc_index; + // CAN modes + CAN_MODE can_mode; + uint8_t uavcan_esc_index; - // Application to use - app_use app_to_use; + // Application to use + app_use app_to_use; - // PPM application settings - ppm_config app_ppm_conf; + // PPM application settings + ppm_config app_ppm_conf; - // ADC application settings - adc_config app_adc_conf; + // ADC application settings + adc_config app_adc_conf; - // UART application settings - uint32_t app_uart_baudrate; + // UART application settings + uint32_t app_uart_baudrate; - // Nunchuk application settings - chuk_config app_chuk_conf; + // Nunchuk application settings + chuk_config app_chuk_conf; - // NRF application settings - nrf_config app_nrf_conf; + // NRF application settings + nrf_config app_nrf_conf; - // Balance application settings - balance_config app_balance_conf; + // Balance application settings + balance_config app_balance_conf; - // IMU Settings - imu_config imu_conf; + // IMU Settings + imu_config imu_conf; } app_configuration; // Communication commands -typedef enum { - COMM_FW_VERSION = 0, - COMM_JUMP_TO_BOOTLOADER, - COMM_ERASE_NEW_APP, - COMM_WRITE_NEW_APP_DATA, - COMM_GET_VALUES, - COMM_SET_DUTY, - COMM_SET_CURRENT, - COMM_SET_CURRENT_BRAKE, - COMM_SET_RPM, - COMM_SET_POS, - COMM_SET_HANDBRAKE, - COMM_SET_DETECT, - COMM_SET_SERVO_POS, - COMM_SET_MCCONF, - COMM_GET_MCCONF, - COMM_GET_MCCONF_DEFAULT, - COMM_SET_APPCONF, - COMM_GET_APPCONF, - COMM_GET_APPCONF_DEFAULT, - COMM_SAMPLE_PRINT, - COMM_TERMINAL_CMD, - COMM_PRINT, - COMM_ROTOR_POSITION, - COMM_EXPERIMENT_SAMPLE, - COMM_DETECT_MOTOR_PARAM, - COMM_DETECT_MOTOR_R_L, - COMM_DETECT_MOTOR_FLUX_LINKAGE, - COMM_DETECT_ENCODER, - COMM_DETECT_HALL_FOC, - COMM_REBOOT, - COMM_ALIVE, - COMM_GET_DECODED_PPM, - COMM_GET_DECODED_ADC, - COMM_GET_DECODED_CHUK, - COMM_FORWARD_CAN, - COMM_SET_CHUCK_DATA, - COMM_CUSTOM_APP_DATA, - COMM_NRF_START_PAIRING, - COMM_GPD_SET_FSW, - COMM_GPD_BUFFER_NOTIFY, - COMM_GPD_BUFFER_SIZE_LEFT, - COMM_GPD_FILL_BUFFER, - COMM_GPD_OUTPUT_SAMPLE, - COMM_GPD_SET_MODE, - COMM_GPD_FILL_BUFFER_INT8, - COMM_GPD_FILL_BUFFER_INT16, - COMM_GPD_SET_BUFFER_INT_SCALE, - COMM_GET_VALUES_SETUP, - COMM_SET_MCCONF_TEMP, - COMM_SET_MCCONF_TEMP_SETUP, - COMM_GET_VALUES_SELECTIVE, - COMM_GET_VALUES_SETUP_SELECTIVE, - COMM_EXT_NRF_PRESENT, - COMM_EXT_NRF_ESB_SET_CH_ADDR, - COMM_EXT_NRF_ESB_SEND_DATA, - COMM_EXT_NRF_ESB_RX_DATA, - COMM_EXT_NRF_SET_ENABLED, - COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, - COMM_DETECT_APPLY_ALL_FOC, - COMM_JUMP_TO_BOOTLOADER_ALL_CAN, - COMM_ERASE_NEW_APP_ALL_CAN, - COMM_WRITE_NEW_APP_DATA_ALL_CAN, - COMM_PING_CAN, - COMM_APP_DISABLE_OUTPUT, - COMM_TERMINAL_CMD_SYNC, - COMM_GET_IMU_DATA, - COMM_BM_CONNECT, - COMM_BM_ERASE_FLASH_ALL, - COMM_BM_WRITE_FLASH, - COMM_BM_REBOOT, - COMM_BM_DISCONNECT, - COMM_BM_MAP_PINS_DEFAULT, - COMM_BM_MAP_PINS_NRF5X, - COMM_ERASE_BOOTLOADER, - COMM_ERASE_BOOTLOADER_ALL_CAN, - COMM_PLOT_INIT, - COMM_PLOT_DATA, - COMM_PLOT_ADD_GRAPH, - COMM_PLOT_SET_GRAPH, - COMM_GET_DECODED_BALANCE, - COMM_BM_MEM_READ, - COMM_WRITE_NEW_APP_DATA_LZO, - COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, - COMM_BM_WRITE_FLASH_LZO, - COMM_SET_CURRENT_REL, - COMM_CAN_FWD_FRAME, - COMM_SET_BATTERY_CUT, - COMM_SET_BLE_NAME, - COMM_SET_BLE_PIN, - COMM_SET_CAN_MODE, - COMM_GET_IMU_CALIBRATION +typedef enum +{ + COMM_FW_VERSION = 0, + COMM_JUMP_TO_BOOTLOADER, + COMM_ERASE_NEW_APP, + COMM_WRITE_NEW_APP_DATA, + COMM_GET_VALUES, + COMM_SET_DUTY, + COMM_SET_CURRENT, + COMM_SET_CURRENT_BRAKE, + COMM_SET_RPM, + COMM_SET_POS, + COMM_SET_HANDBRAKE, + COMM_SET_DETECT, + COMM_SET_SERVO_POS, + COMM_SET_MCCONF, + COMM_GET_MCCONF, + COMM_GET_MCCONF_DEFAULT, + COMM_SET_APPCONF, + COMM_GET_APPCONF, + COMM_GET_APPCONF_DEFAULT, + COMM_SAMPLE_PRINT, + COMM_TERMINAL_CMD, + COMM_PRINT, + COMM_ROTOR_POSITION, + COMM_EXPERIMENT_SAMPLE, + COMM_DETECT_MOTOR_PARAM, + COMM_DETECT_MOTOR_R_L, + COMM_DETECT_MOTOR_FLUX_LINKAGE, + COMM_DETECT_ENCODER, + COMM_DETECT_HALL_FOC, + COMM_REBOOT, + COMM_ALIVE, + COMM_GET_DECODED_PPM, + COMM_GET_DECODED_ADC, + COMM_GET_DECODED_CHUK, + COMM_FORWARD_CAN, + COMM_SET_CHUCK_DATA, + COMM_CUSTOM_APP_DATA, + COMM_NRF_START_PAIRING, + COMM_GPD_SET_FSW, + COMM_GPD_BUFFER_NOTIFY, + COMM_GPD_BUFFER_SIZE_LEFT, + COMM_GPD_FILL_BUFFER, + COMM_GPD_OUTPUT_SAMPLE, + COMM_GPD_SET_MODE, + COMM_GPD_FILL_BUFFER_INT8, + COMM_GPD_FILL_BUFFER_INT16, + COMM_GPD_SET_BUFFER_INT_SCALE, + COMM_GET_VALUES_SETUP, + COMM_SET_MCCONF_TEMP, + COMM_SET_MCCONF_TEMP_SETUP, + COMM_GET_VALUES_SELECTIVE, + COMM_GET_VALUES_SETUP_SELECTIVE, + COMM_EXT_NRF_PRESENT, + COMM_EXT_NRF_ESB_SET_CH_ADDR, + COMM_EXT_NRF_ESB_SEND_DATA, + COMM_EXT_NRF_ESB_RX_DATA, + COMM_EXT_NRF_SET_ENABLED, + COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + COMM_DETECT_APPLY_ALL_FOC, + COMM_JUMP_TO_BOOTLOADER_ALL_CAN, + COMM_ERASE_NEW_APP_ALL_CAN, + COMM_WRITE_NEW_APP_DATA_ALL_CAN, + COMM_PING_CAN, + COMM_APP_DISABLE_OUTPUT, + COMM_TERMINAL_CMD_SYNC, + COMM_GET_IMU_DATA, + COMM_BM_CONNECT, + COMM_BM_ERASE_FLASH_ALL, + COMM_BM_WRITE_FLASH, + COMM_BM_REBOOT, + COMM_BM_DISCONNECT, + COMM_BM_MAP_PINS_DEFAULT, + COMM_BM_MAP_PINS_NRF5X, + COMM_ERASE_BOOTLOADER, + COMM_ERASE_BOOTLOADER_ALL_CAN, + COMM_PLOT_INIT, + COMM_PLOT_DATA, + COMM_PLOT_ADD_GRAPH, + COMM_PLOT_SET_GRAPH, + COMM_GET_DECODED_BALANCE, + COMM_BM_MEM_READ, + COMM_WRITE_NEW_APP_DATA_LZO, + COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, + COMM_BM_WRITE_FLASH_LZO, + COMM_SET_CURRENT_REL, + COMM_CAN_FWD_FRAME, + COMM_SET_BATTERY_CUT, + COMM_SET_BLE_NAME, + COMM_SET_BLE_PIN, + COMM_SET_CAN_MODE, + COMM_GET_IMU_CALIBRATION } COMM_PACKET_ID; // CAN commands -typedef enum { - CAN_PACKET_SET_DUTY = 0, - CAN_PACKET_SET_CURRENT, - CAN_PACKET_SET_CURRENT_BRAKE, - CAN_PACKET_SET_RPM, - CAN_PACKET_SET_POS, - CAN_PACKET_FILL_RX_BUFFER, - CAN_PACKET_FILL_RX_BUFFER_LONG, - CAN_PACKET_PROCESS_RX_BUFFER, - CAN_PACKET_PROCESS_SHORT_BUFFER, - CAN_PACKET_STATUS, - CAN_PACKET_SET_CURRENT_REL, - CAN_PACKET_SET_CURRENT_BRAKE_REL, - CAN_PACKET_SET_CURRENT_HANDBRAKE, - CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, - CAN_PACKET_STATUS_2, - CAN_PACKET_STATUS_3, - CAN_PACKET_STATUS_4, - CAN_PACKET_PING, - CAN_PACKET_PONG, - CAN_PACKET_DETECT_APPLY_ALL_FOC, - CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, - CAN_PACKET_CONF_CURRENT_LIMITS, - CAN_PACKET_CONF_STORE_CURRENT_LIMITS, - CAN_PACKET_CONF_CURRENT_LIMITS_IN, - CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, - CAN_PACKET_CONF_FOC_ERPMS, - CAN_PACKET_CONF_STORE_FOC_ERPMS, - CAN_PACKET_STATUS_5, - CAN_PACKET_POLL_TS5700N8501_STATUS, - CAN_PACKET_CONF_BATTERY_CUT, - CAN_PACKET_CONF_STORE_BATTERY_CUT, - CAN_PACKET_SHUTDOWN +typedef enum +{ + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT, + CAN_PACKET_SET_CURRENT_BRAKE, + CAN_PACKET_SET_RPM, + CAN_PACKET_SET_POS, + CAN_PACKET_FILL_RX_BUFFER, + CAN_PACKET_FILL_RX_BUFFER_LONG, + CAN_PACKET_PROCESS_RX_BUFFER, + CAN_PACKET_PROCESS_SHORT_BUFFER, + CAN_PACKET_STATUS, + CAN_PACKET_SET_CURRENT_REL, + CAN_PACKET_SET_CURRENT_BRAKE_REL, + CAN_PACKET_SET_CURRENT_HANDBRAKE, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, + CAN_PACKET_STATUS_2, + CAN_PACKET_STATUS_3, + CAN_PACKET_STATUS_4, + CAN_PACKET_PING, + CAN_PACKET_PONG, + CAN_PACKET_DETECT_APPLY_ALL_FOC, + CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, + CAN_PACKET_CONF_CURRENT_LIMITS, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS, + CAN_PACKET_CONF_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_FOC_ERPMS, + CAN_PACKET_CONF_STORE_FOC_ERPMS, + CAN_PACKET_STATUS_5, + CAN_PACKET_POLL_TS5700N8501_STATUS, + CAN_PACKET_CONF_BATTERY_CUT, + CAN_PACKET_CONF_STORE_BATTERY_CUT, + CAN_PACKET_SHUTDOWN } CAN_PACKET_ID; // Logged fault data -typedef struct { - uint8_t motor; - mc_fault_code fault; - float current; - float current_filtered; - float voltage; - float gate_driver_voltage; - float duty; - float rpm; - int tacho; - int cycles_running; - int tim_val_samp; - int tim_current_samp; - int tim_top; - int comm_step; - float temperature; - int drv8301_faults; +typedef struct +{ + uint8_t motor; + mc_fault_code fault; + float current; + float current_filtered; + float voltage; + float gate_driver_voltage; + float duty; + float rpm; + int tacho; + int cycles_running; + int tim_val_samp; + int tim_current_samp; + int tim_top; + int comm_step; + float temperature; + int drv8301_faults; } fault_data; // External LED state -typedef enum { - LED_EXT_OFF = 0, - LED_EXT_NORMAL, - LED_EXT_BRAKE, - LED_EXT_TURN_LEFT, - LED_EXT_TURN_RIGHT, - LED_EXT_BRAKE_TURN_LEFT, - LED_EXT_BRAKE_TURN_RIGHT, - LED_EXT_BATT +typedef enum +{ + LED_EXT_OFF = 0, + LED_EXT_NORMAL, + LED_EXT_BRAKE, + LED_EXT_TURN_LEFT, + LED_EXT_TURN_RIGHT, + LED_EXT_BRAKE_TURN_LEFT, + LED_EXT_BRAKE_TURN_RIGHT, + LED_EXT_BATT } LED_EXT_STATE; -typedef struct { - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; - bool rev_has_state; - bool is_rev; +typedef struct +{ + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; + bool rev_has_state; + bool is_rev; } chuck_data; -typedef struct { - int id; - systime_t rx_time; - float rpm; - float current; - float duty; +typedef struct +{ + int id; + systime_t rx_time; + float rpm; + float current; + float duty; } can_status_msg; -typedef struct { - int id; - systime_t rx_time; - float amp_hours; - float amp_hours_charged; +typedef struct +{ + int id; + systime_t rx_time; + float amp_hours; + float amp_hours_charged; } can_status_msg_2; -typedef struct { - int id; - systime_t rx_time; - float watt_hours; - float watt_hours_charged; +typedef struct +{ + int id; + systime_t rx_time; + float watt_hours; + float watt_hours_charged; } can_status_msg_3; -typedef struct { - int id; - systime_t rx_time; - float temp_fet; - float temp_motor; - float current_in; - float pid_pos_now; +typedef struct +{ + int id; + systime_t rx_time; + float temp_fet; + float temp_motor; + float current_in; + float pid_pos_now; } can_status_msg_4; -typedef struct { - int id; - systime_t rx_time; - float v_in; - int32_t tacho_value; +typedef struct +{ + int id; + systime_t rx_time; + float v_in; + int32_t tacho_value; } can_status_msg_5; -typedef struct { - uint8_t js_x; - uint8_t js_y; - bool bt_c; - bool bt_z; - bool bt_push; - bool rev_has_state; - bool is_rev; - float vbat; +typedef struct +{ + uint8_t js_x; + uint8_t js_y; + bool bt_c; + bool bt_z; + bool bt_push; + bool rev_has_state; + bool is_rev; + float vbat; } mote_state; -typedef enum { - MOTE_PACKET_BATT_LEVEL = 0, - MOTE_PACKET_BUTTONS, - MOTE_PACKET_ALIVE, - MOTE_PACKET_FILL_RX_BUFFER, - MOTE_PACKET_FILL_RX_BUFFER_LONG, - MOTE_PACKET_PROCESS_RX_BUFFER, - MOTE_PACKET_PROCESS_SHORT_BUFFER, - MOTE_PACKET_PAIRING_INFO +typedef enum +{ + MOTE_PACKET_BATT_LEVEL = 0, + MOTE_PACKET_BUTTONS, + MOTE_PACKET_ALIVE, + MOTE_PACKET_FILL_RX_BUFFER, + MOTE_PACKET_FILL_RX_BUFFER_LONG, + MOTE_PACKET_PROCESS_RX_BUFFER, + MOTE_PACKET_PROCESS_SHORT_BUFFER, + MOTE_PACKET_PAIRING_INFO } MOTE_PACKET; -typedef struct { - float v_in; - float temp_mos; - float temp_mos_1; - float temp_mos_2; - float temp_mos_3; - float temp_motor; - float current_motor; - float current_in; - float id; - float iq; - float rpm; - float duty_now; - float amp_hours; - float amp_hours_charged; - float watt_hours; - float watt_hours_charged; - int tachometer; - int tachometer_abs; - float position; - mc_fault_code fault_code; - int vesc_id; - float vd; - float vq; +typedef struct +{ + float v_in; + float temp_mos; + float temp_mos_1; + float temp_mos_2; + float temp_mos_3; + float temp_motor; + float current_motor; + float current_in; + float id; + float iq; + float rpm; + float duty_now; + float amp_hours; + float amp_hours_charged; + float watt_hours; + float watt_hours_charged; + int tachometer; + int tachometer_abs; + float position; + mc_fault_code fault_code; + int vesc_id; + float vd; + float vq; } mc_values; -typedef enum { - NRF_PAIR_STARTED = 0, - NRF_PAIR_OK, - NRF_PAIR_FAIL +typedef enum +{ + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL } NRF_PAIR_RES; // Orientation data -typedef struct { - float q0; - float q1; - float q2; - float q3; - float integralFBx; - float integralFBy; - float integralFBz; - float accMagP; - int initialUpdateDone; +typedef struct +{ + float q0; + float q1; + float q2; + float q3; + float integralFBx; + float integralFBy; + float integralFBz; + float accMagP; + int initialUpdateDone; } ATTITUDE_INFO; // Custom EEPROM variables typedef union { - uint32_t as_u32; - int32_t as_i32; - float as_float; + uint32_t as_u32; + int32_t as_i32; + float as_float; } eeprom_var; -#define EEPROM_VARS_HW 64 -#define EEPROM_VARS_CUSTOM 64 - -typedef struct { - float ah_tot; - float ah_charge_tot; - float wh_tot; - float wh_charge_tot; - float current_tot; - float current_in_tot; - uint8_t num_vescs; +#define EEPROM_VARS_HW 64 +#define EEPROM_VARS_CUSTOM 64 + +typedef struct +{ + float ah_tot; + float ah_charge_tot; + float wh_tot; + float wh_charge_tot; + float current_tot; + float current_in_tot; + uint8_t num_vescs; } setup_values; -} -#endif /* DATATYPES_H_ */ +} // namespace vesc_driver +#endif // VESC_DRIVER__DATATYPES_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index fa1fc3c..aa47cc8 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -121,18 +121,18 @@ class VescPacketFWVersion : public VescPacket int fwMajor() const; int fwMinor() const; - std::string hwname() const; - const uint8_t* uuid() const; + std::string hwname() const; + const uint8_t * uuid() const; bool paired() const; uint8_t devVersion() const; - private: +private: int minor_; int major_; std::string hwname_; bool paired_; - uint8_t uuid_[12]; - uint8_t devVersion_; + uint8_t uuid_[12]; + uint8_t devVersion_; }; class VescPacketRequestFWVersion : public VescPacket @@ -148,35 +148,34 @@ class VescPacketValues : public VescPacket public: explicit VescPacketValues(std::shared_ptr raw); -double temp_fet() const; -double temp_motor() const; -double avg_motor_current() const; -double avg_input_current() const; -double avg_id() const; -double avg_iq() const ; -double duty_cycle_now() const; -double rpm() const; -double duty_now() const; -double v_in() const; -double amp_hours() const; -double amp_hours_charged() const; -double watt_hours() const; -double watt_hours_charged() const; -int32_t tachometer() const; -int32_t tachometer_abs() const; -int fault_code() const; -double pid_pos_now() const; -int32_t controller_id() const; - -double temp_mos1() const; -double temp_mos2() const; -double temp_mos3() const; -double avg_vd() const; -double avg_vq() const; - -int32_t numVescs() const; -double watt_battery_left() const; - + double temp_fet() const; + double temp_motor() const; + double avg_motor_current() const; + double avg_input_current() const; + double avg_id() const; + double avg_iq() const; + double duty_cycle_now() const; + double rpm() const; + double duty_now() const; + double v_in() const; + double amp_hours() const; + double amp_hours_charged() const; + double watt_hours() const; + double watt_hours_charged() const; + int32_t tachometer() const; + int32_t tachometer_abs() const; + int fault_code() const; + double pid_pos_now() const; + int32_t controller_id() const; + + double temp_mos1() const; + double temp_mos2() const; + double temp_mos3() const; + double avg_vd() const; + double avg_vq() const; + + int32_t numVescs() const; + double watt_battery_left() const; }; class VescPacketRequestValues : public VescPacket diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 700a74f..a9fbc7f 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -162,54 +162,56 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa auto state_msg = VescStateStamped(); state_msg.header.stamp = now(); - + state_msg.state.voltage_input = values->v_in(); state_msg.state.current_motor = values->avg_motor_current(); state_msg.state.current_input = values->avg_input_current(); - state_msg.state.avg_id = values->avg_id(); - state_msg.state.avg_iq = values->avg_iq(); - state_msg.state.duty_cycle = values->duty_cycle_now(); - state_msg.state.speed = values->rpm(); - + state_msg.state.avg_id = values->avg_id(); + state_msg.state.avg_iq = values->avg_iq(); + state_msg.state.duty_cycle = values->duty_cycle_now(); + state_msg.state.speed = values->rpm(); + state_msg.state.charge_drawn = values->amp_hours(); state_msg.state.charge_regen = values->amp_hours_charged(); state_msg.state.energy_drawn = values->watt_hours(); state_msg.state.energy_regen = values->watt_hours_charged(); state_msg.state.displacement = values->tachometer(); state_msg.state.distance_traveled = values->tachometer_abs(); - state_msg.state.fault_code = values->fault_code(); + state_msg.state.fault_code = values->fault_code(); - state_msg.state.pid_pos_now = values->pid_pos_now(); - state_msg.state.controller_id = values->controller_id(); + state_msg.state.pid_pos_now = values->pid_pos_now(); + state_msg.state.controller_id = values->controller_id(); - state_msg.state.ntc_temp_mos1 = values->temp_mos1(); - state_msg.state.ntc_temp_mos2 = values->temp_mos2(); - state_msg.state.ntc_temp_mos3 = values->temp_mos3(); - state_msg.state.avg_vd = values->avg_vd(); - state_msg.state.avg_vq = values->avg_vq(); + state_msg.state.ntc_temp_mos1 = values->temp_mos1(); + state_msg.state.ntc_temp_mos2 = values->temp_mos2(); + state_msg.state.ntc_temp_mos3 = values->temp_mos3(); + state_msg.state.avg_vd = values->avg_vd(); + state_msg.state.avg_vq = values->avg_vq(); state_pub_->publish(state_msg); } else if (packet->name() == "FWVersion") { - std::shared_ptr fw_version = + std::shared_ptr fw_version = std::dynamic_pointer_cast(packet); // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); RCLCPP_INFO( - get_logger(), - "-=%s=- hardware paired %d", - fw_version->hwname().c_str(), - fw_version->paired() - ); + get_logger(), + "-=%s=- hardware paired %d", + fw_version->hwname().c_str(), + fw_version->paired() + ); } - - auto& clk = *this->get_clock(); + + auto & clk = *this->get_clock(); RCLCPP_INFO_THROTTLE( - get_logger(), - clk, - 5000, - "%s packet received",packet->name().c_str()); + get_logger(), + clk, + 5000, + "%s packet received", + packet->name().c_str() + ); } void VescDriver::vescErrorCallback(const std::string & error) diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 7ebf0cb..1cb9980 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -34,10 +34,11 @@ #include #include #include +#include #include "vesc_driver/datatypes.hpp" #include "vesc_driver/vesc_packet_factory.hpp" -#include + namespace vesc_driver { @@ -101,31 +102,22 @@ VescPacket::VescPacket(const std::string & name, std::shared_ptr raw) VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : VescPacket("FWVersion", raw) { - major_ = *(payload_.first + 1); - minor_ = *(payload_.first + 2); - int j=0; - - - for(int i=0;*(payload_.first+3+i)!=0x00;i++){ - j = i; - hwname_ += *(payload_.first+3+i); - } + minor_ = *(payload_.first + 2); + int j = 0; - j++; - - - - - for(int u=0;u<12;u++){ - uuid_[u] = *(payload_.first+3+j+1+u); - + for (int i = 0; *(payload_.first + 3 + i) != 0x00; i++) { + j = i; + hwname_ += *(payload_.first + 3 + i); } - + j++; - paired_ = *(payload_.first+3+j+1+12); - devVersion_ = *(payload_.first+3+j+2+12+1); + for (int u = 0; u < 12; u++) { + uuid_[u] = *(payload_.first + 3 + j + 1 + u); + } + paired_ = *(payload_.first + 3 + j + 1 + 12); + devVersion_ = *(payload_.first + 3 + j + 2 + 12 + 1); } int VescPacketFWVersion::fwMajor() const @@ -138,22 +130,21 @@ int VescPacketFWVersion::fwMinor() const return minor_; } -std::string VescPacketFWVersion::hwname() const +std::string VescPacketFWVersion::hwname() const { - - return hwname_.c_str(); + return hwname_.c_str(); } -const uint8_t* VescPacketFWVersion::uuid() const +const uint8_t * VescPacketFWVersion::uuid() const { return uuid_; } -bool VescPacketFWVersion::paired() const +bool VescPacketFWVersion::paired() const { - return paired_; + return paired_; } -uint8_t VescPacketFWVersion::devVersion() const +uint8_t VescPacketFWVersion::devVersion() const { - return devVersion_; + return devVersion_; } REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) @@ -173,141 +164,164 @@ VescPacketValues::VescPacketValues(std::shared_ptr raw) : VescPacket("Values", raw) { } -double VescPacketValues::temp_fet() const{ - int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); +double VescPacketValues::temp_fet() const +{ + int16_t v = static_cast( + (static_cast(*(payload_.first + 1)) << 8) + + (static_cast(*(payload_.first + 2)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::temp_motor() const{ - int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - static_cast(*(payload_.first + 4))); +double VescPacketValues::temp_motor() const +{ + int16_t v = static_cast( + (static_cast(*(payload_.first + 3)) << 8) + + (static_cast(*(payload_.first + 4)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::avg_motor_current() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + - (static_cast(*(payload_.first + 6)) << 16) + - (static_cast(*(payload_.first + 7)) << 8) + - static_cast(*(payload_.first + 8)) - ); +double VescPacketValues::avg_motor_current() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 5)) << 24) + + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + + (static_cast(*(payload_.first + 8)) ) + ); return static_cast(v) / 100.0; - } -double VescPacketValues::avg_input_current() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + - (static_cast(*(payload_.first + 10)) << 16) + - (static_cast(*(payload_.first + 11)) << 8) + - static_cast(*(payload_.first + 12)) - ); +double VescPacketValues::avg_input_current() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 9)) << 24) + + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + (static_cast(*(payload_.first + 12)) ) + ); return static_cast(v) / 100.0; - } -double VescPacketValues::avg_id() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 13)) << 24) + - (static_cast(*(payload_.first + 14)) << 16) + - (static_cast(*(payload_.first + 15)) << 8) + - static_cast(*(payload_.first + 16)) - ); +double VescPacketValues::avg_id() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 13)) << 24) + + (static_cast(*(payload_.first + 14)) << 16) + + (static_cast(*(payload_.first + 15)) << 8) + + (static_cast(*(payload_.first + 16)) ) + ); return static_cast(v) / 100.0; - } -double VescPacketValues::avg_iq() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 17)) << 24) + - (static_cast(*(payload_.first + 18)) << 16) + - (static_cast(*(payload_.first + 19)) << 8) + - static_cast(*(payload_.first + 20)) - ); +double VescPacketValues::avg_iq() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 17)) << 24) + + (static_cast(*(payload_.first + 18)) << 16) + + (static_cast(*(payload_.first + 19)) << 8) + + (static_cast(*(payload_.first + 20)) ) + ); return static_cast(v) / 100.0; - } double VescPacketValues::duty_cycle_now() const { - int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); + int16_t v = static_cast( + (static_cast(*(payload_.first + 21)) << 8) + + (static_cast(*(payload_.first + 22)) ) + ); return static_cast(v) / 1000.0; } -double VescPacketValues::rpm() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26)) - ); +double VescPacketValues::rpm() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 23)) << 24) + + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + + (static_cast(*(payload_.first + 26)) ) + ); return static_cast(v) / 1.0; - } double VescPacketValues::v_in() const { - int16_t v = static_cast((static_cast(*(payload_.first + 27)) << 8) + - static_cast(*(payload_.first + 28))); - return static_cast(v) /10.0; + int16_t v = static_cast( + (static_cast(*(payload_.first + 27)) << 8) + + (static_cast(*(payload_.first + 28)) ) + ); + return static_cast(v) / 10.0; } -double VescPacketValues::amp_hours() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 29)) << 24) + - (static_cast(*(payload_.first + 30)) << 16) + - (static_cast(*(payload_.first + 31)) << 8) + - static_cast(*(payload_.first + 32)) - ); +double VescPacketValues::amp_hours() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 29)) << 24) + + (static_cast(*(payload_.first + 30)) << 16) + + (static_cast(*(payload_.first + 31)) << 8) + + (static_cast(*(payload_.first + 32)) ) + ); return static_cast(v) / 1e4; - } -double VescPacketValues::amp_hours_charged() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 33)) << 24) + - (static_cast(*(payload_.first + 34)) << 16) + - (static_cast(*(payload_.first + 35)) << 8) + - static_cast(*(payload_.first + 36)) - ); +double VescPacketValues::amp_hours_charged() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 33)) << 24) + + (static_cast(*(payload_.first + 34)) << 16) + + (static_cast(*(payload_.first + 35)) << 8) + + (static_cast(*(payload_.first + 36)) ) + ); return static_cast(v) / 1e4; - } -double VescPacketValues::watt_hours() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 37)) << 24) + - (static_cast(*(payload_.first + 38)) << 16) + - (static_cast(*(payload_.first + 39)) << 8) + - static_cast(*(payload_.first + 40)) - ); +double VescPacketValues::watt_hours() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 37)) << 24) + + (static_cast(*(payload_.first + 38)) << 16) + + (static_cast(*(payload_.first + 39)) << 8) + + (static_cast(*(payload_.first + 40)) ) + ); return static_cast(v) / 1e4; - } -double VescPacketValues::watt_hours_charged() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 41)) << 24) + - (static_cast(*(payload_.first + 42)) << 16) + - (static_cast(*(payload_.first + 43)) << 8) + - static_cast(*(payload_.first + 44)) - ); +double VescPacketValues::watt_hours_charged() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 41)) << 24) + + (static_cast(*(payload_.first + 42)) << 16) + + (static_cast(*(payload_.first + 43)) << 8) + + (static_cast(*(payload_.first + 44)) ) + ); return static_cast(v) / 1e4; - } int32_t VescPacketValues::tachometer() const { - int32_t v = static_cast((static_cast(*(payload_.first + 45)) << 24) + - (static_cast(*(payload_.first + 46)) << 16) + - (static_cast(*(payload_.first + 47)) << 8) + - static_cast(*(payload_.first + 48))); + int32_t v = static_cast( + (static_cast(*(payload_.first + 45)) << 24) + + (static_cast(*(payload_.first + 46)) << 16) + + (static_cast(*(payload_.first + 47)) << 8) + + (static_cast(*(payload_.first + 48)) ) + ); return static_cast(v); } int32_t VescPacketValues::tachometer_abs() const { - int32_t v = static_cast((static_cast(*(payload_.first + 49)) << 24) + - (static_cast(*(payload_.first + 50)) << 16) + - (static_cast(*(payload_.first + 51)) << 8) + - static_cast(*(payload_.first + 52))); + int32_t v = static_cast( + (static_cast(*(payload_.first + 49)) << 24) + + (static_cast(*(payload_.first + 50)) << 16) + + (static_cast(*(payload_.first + 51)) << 8) + + (static_cast(*(payload_.first + 52)) ) + ); return static_cast(v); } @@ -319,67 +333,70 @@ int VescPacketValues::fault_code() const double VescPacketValues::pid_pos_now() const { - int32_t v = static_cast((static_cast(*(payload_.first + 54)) << 24) + - (static_cast(*(payload_.first + 55)) << 16) + - (static_cast(*(payload_.first + 56)) << 8) + - static_cast(*(payload_.first + 57)) - ); + int32_t v = static_cast( + (static_cast(*(payload_.first + 54)) << 24) + + (static_cast(*(payload_.first + 55)) << 16) + + (static_cast(*(payload_.first + 56)) << 8) + + (static_cast(*(payload_.first + 57)) ) + ); return static_cast(v) / 1e6; - } int32_t VescPacketValues::controller_id() const { - //int32_t v = static_cast((static_cast(*(payload_.first + 58)) << 24) + - // (static_cast(*(payload_.first + 59)) << 16) + - // (static_cast(*(payload_.first + 60)) << 8) + - // static_cast(*(payload_.first + 61))); -// - int32_t v = static_cast((static_cast(*(payload_.first + 58) ))); + int32_t v = static_cast((static_cast(*(payload_.first + 58) ))); return static_cast(v); } double VescPacketValues::temp_mos1() const { - int16_t v = static_cast((static_cast(*(payload_.first + 59)) << 8) + - static_cast(*(payload_.first + 60))); + 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 + 61)) << 8) + - static_cast(*(payload_.first + 62))); + 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))); + int16_t v = static_cast( + (static_cast(*(payload_.first + 63)) << 8) + + (static_cast(*(payload_.first + 64)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::avg_vd() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 65)) << 24) + - (static_cast(*(payload_.first + 66)) << 16) + - (static_cast(*(payload_.first + 67)) << 8) + - static_cast(*(payload_.first + 68)) - ); +double VescPacketValues::avg_vd() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 65)) << 24) + + (static_cast(*(payload_.first + 66)) << 16) + + (static_cast(*(payload_.first + 67)) << 8) + + (static_cast(*(payload_.first + 68)) ) + ); return static_cast(v) / 1e3; - } -double VescPacketValues::avg_vq() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 69)) << 24) + - (static_cast(*(payload_.first + 70)) << 16) + - (static_cast(*(payload_.first + 71)) << 8) + - static_cast(*(payload_.first + 72)) - ); +double VescPacketValues::avg_vq() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 69)) << 24) + + (static_cast(*(payload_.first + 70)) << 16) + + (static_cast(*(payload_.first + 71)) << 8) + + (static_cast(*(payload_.first + 72)) ) + ); return static_cast(v) / 1e3; - } From acd2bf45a1298509d695dbaaff2d266da4f0da13 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Thu, 3 Jun 2021 21:14:40 +0200 Subject: [PATCH 28/72] removed old methods --- vesc_driver/include/vesc_driver/vesc_packet.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index aa47cc8..74ccd72 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -173,9 +173,6 @@ class VescPacketValues : public VescPacket double temp_mos3() const; double avg_vd() const; double avg_vq() const; - - int32_t numVescs() const; - double watt_battery_left() const; }; class VescPacketRequestValues : public VescPacket From 21d75d331d4c727e35bd59c36e33b7b6d3d47c9c Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Fri, 4 Jun 2021 00:05:05 +0200 Subject: [PATCH 29/72] removed commented-out code --- vesc_driver/src/vesc_interface.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 21fec38..728c820 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -134,8 +134,6 @@ void VescInterface::Impl::rxThread() int bytes_to_read = std::min(bytes_needed, 4096); { - // removed temporarily because it causes a deadlock - // std::lock_guard lock(serial_mutex_); boost::system::error_code ec; const size_t bytes_read = boost::asio::read( @@ -148,7 +146,7 @@ void VescInterface::Impl::rxThread() if (ec.value() > 0) { std::ostringstream ss; - ss << "Serial port comunication error " << ec << ec << std::endl; + ss << "Serial port comunication error " << std::endl; ss << "failed " << ec.failed() << std::endl; ss << ec.value() << std::endl; ss << ec.category().name() << std::endl; @@ -242,8 +240,6 @@ void VescInterface::disconnect() // bring down read thread impl_->rx_thread_run_ = false; impl_->rx_thread_->join(); - - // std::lock_guard lock(impl_->serial_mutex_); impl_->serial_port_.close(); } } From 556abea7404da11ee6820e76b835e8c41d84d767 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Thu, 5 Aug 2021 10:29:28 -1000 Subject: [PATCH 30/72] minor coding practice updates --- vesc_driver/CMakeLists.txt | 17 +- .../include/vesc_driver/vesc_driver.hpp | 1 + vesc_driver/launch/vesc_node.launch.py | 0 vesc_driver/src/vesc_interface_old.cpp | 293 ------------------ 4 files changed, 9 insertions(+), 302 deletions(-) create mode 100644 vesc_driver/launch/vesc_node.launch.py delete mode 100644 vesc_driver/src/vesc_interface_old.cpp diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 23abfb7..2e6f1bb 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -2,16 +2,15 @@ cmake_minimum_required(VERSION 3.5) project(vesc_driver) # Set minimum C++ standard to C++17 -#if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") -# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") -#elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") -# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") -# set(CMAKE_CXX_STANDARD 17) -#endif() -set(CMAKE_CXX_STANDARD 17) +if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") + message(STATUS "Changing CXX_STANDARD from C++98 to C++17") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") +elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") + message(STATUS "Changing CXX_STANDARD from C++98 to C++17") + set(CMAKE_CXX_STANDARD 17) +endif() + find_package(ament_cmake_auto REQUIRED) -find_package(serial_driver REQUIRED) ament_auto_find_build_dependencies() find_package(Threads) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index 3a0cfa7..2169fcb 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -38,6 +38,7 @@ #include #include +#include #include "vesc_driver/vesc_interface.hpp" #include "vesc_driver/vesc_packet.hpp" diff --git a/vesc_driver/launch/vesc_node.launch.py b/vesc_driver/launch/vesc_node.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/vesc_driver/src/vesc_interface_old.cpp b/vesc_driver/src/vesc_interface_old.cpp deleted file mode 100644 index 0b0c20d..0000000 --- a/vesc_driver/src/vesc_interface_old.cpp +++ /dev/null @@ -1,293 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// -*- mode:c++; fill-column: 100; -*- - -#include "vesc_driver/vesc_interface.hpp" -#include "vesc_driver/vesc_packet_factory.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace vesc_driver -{ - -class VescInterface::Impl -{ -public: - Impl() - : io_service_(), - serial_port_(io_service_) - {} - - void rxThread(); - - std::unique_ptr rx_thread_; - bool rx_thread_run_; - PacketHandlerFunction packet_handler_; - ErrorHandlerFunction error_handler_; - boost::asio::io_service io_service_; - boost::asio::serial_port serial_port_; - std::mutex serial_mutex_; -}; - -void VescInterface::Impl::rxThread() -{ - Buffer buffer; - buffer.reserve(4096); - - while (rx_thread_run_) { - int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer.empty()) { - // search buffer for valid packet(s) - Buffer::iterator iter(buffer.begin()); - Buffer::iterator iter_begin(buffer.begin()); - while (iter != buffer.end()) { - // check if valid start-of-frame character - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || - VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) - { - // good start, now attempt to create packet - std::string error; - VescPacketConstPtr packet = - VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); - if (packet) { - // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << - std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } - // call packet handler - packet_handler_(packet); - // update state - iter = iter + packet->frame().size(); - iter_begin = iter; - // continue to look for another frame in buffer - continue; - } else if (bytes_needed > 0) { - // need more data, break out of while loop - break; // for (iter_sof... - } else { - // else, this was not a packet, move on to next byte - error_handler_(error); - } - } - - iter++; - } - - // if iter is at the end of the buffer, more bytes are needed - if (iter == buffer.end()) { - bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - } - - // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } - buffer.erase(buffer.begin(), iter); - } - - // attempt to read at least bytes_needed bytes from the serial port - int bytes_to_read = std::max(bytes_needed, 4096); - - { - std::lock_guard lock(serial_mutex_); - - const size_t bytes_read = boost::asio::read( - serial_port_, - boost::asio::buffer(buffer, buffer.size()), - boost::asio::transfer_exactly(bytes_to_read)); - - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { - error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); - } - } - - // Only attempt to read every 10 ms - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } -} - -VescInterface::VescInterface( - const std::string & port, - const PacketHandlerFunction & packet_handler, - const ErrorHandlerFunction & error_handler) -: impl_(new Impl()) -{ - setPacketHandler(packet_handler); - setErrorHandler(error_handler); - // attempt to conect if the port is specified - if (!port.empty()) { - connect(port); - } -} - -VescInterface::~VescInterface() -{ - disconnect(); -} - -void VescInterface::setPacketHandler(const PacketHandlerFunction & handler) -{ - // todo - definately need mutex - impl_->packet_handler_ = handler; -} - -void VescInterface::setErrorHandler(const ErrorHandlerFunction & handler) -{ - // todo - definately need mutex - impl_->error_handler_ = handler; -} - -void VescInterface::connect(const std::string & port) -{ - // todo - mutex? - - if (isConnected()) { - throw SerialException("Already connected to serial port."); - } - - // connect to serial port - try { - impl_->serial_port_.open(port); - impl_->serial_port_.set_option(boost::asio::serial_port_base::baud_rate(115200)); - impl_->serial_port_.set_option( - boost::asio::serial_port::flow_control( - boost::asio::serial_port::flow_control::none)); - impl_->serial_port_.set_option( - boost::asio::serial_port::parity( - boost::asio::serial_port::parity::none)); - impl_->serial_port_.set_option( - boost::asio::serial_port::stop_bits( - boost::asio::serial_port::stop_bits::one)); - } catch (const std::exception & e) { - std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); - throw SerialException(ss.str().c_str()); - } - - // start up a monitoring thread - impl_->rx_thread_run_ = true; - impl_->rx_thread_ = std::unique_ptr( - new std::thread( - &VescInterface::Impl::rxThread, impl_.get())); -} - -void VescInterface::disconnect() -{ - // todo - mutex? - - if (isConnected()) { - // bring down read thread - impl_->rx_thread_run_ = false; - impl_->rx_thread_->join(); - - std::lock_guard lock(impl_->serial_mutex_); - impl_->serial_port_.close(); - } -} - -bool VescInterface::isConnected() const -{ - std::lock_guard lock(impl_->serial_mutex_); - return impl_->serial_port_.is_open(); -} - -void VescInterface::send(const VescPacket & packet) -{ - try { - std::lock_guard lock(impl_->serial_mutex_); - size_t written = impl_->serial_port_.write_some( - boost::asio::buffer(packet.frame())); - if (written != packet.frame().size()) { - std::stringstream ss; - ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; - throw SerialException(ss.str().c_str()); - } - } catch (const std::exception & e) { - std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); - throw SerialException(ss.str().c_str()); - } -} - -void VescInterface::requestFWVersion() -{ - send(VescPacketRequestFWVersion()); -} - -void VescInterface::requestState() -{ - send(VescPacketRequestValues()); -} - -void VescInterface::setDutyCycle(double duty_cycle) -{ - send(VescPacketSetDuty(duty_cycle)); -} - -void VescInterface::setCurrent(double current) -{ - send(VescPacketSetCurrent(current)); -} - -void VescInterface::setBrake(double brake) -{ - send(VescPacketSetCurrentBrake(brake)); -} - -void VescInterface::setSpeed(double speed) -{ - send(VescPacketSetRPM(speed)); -} - -void VescInterface::setPosition(double position) -{ - send(VescPacketSetPos(position)); -} - -void VescInterface::setServo(double servo) -{ - send(VescPacketSetServoPos(servo)); -} - -} // namespace vesc_driver From 996bda346d149b67d147de989b036e126334cdad Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Thu, 5 Aug 2021 10:42:23 -1000 Subject: [PATCH 31/72] force c++17 --- vesc/package.xml | 1 + vesc_driver/CMakeLists.txt | 1 + vesc_driver/package.xml | 1 + 3 files changed, 3 insertions(+) diff --git a/vesc/package.xml b/vesc/package.xml index d6d68f1..ca4227b 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -13,6 +13,7 @@ https://github.com/f1tenth/vesc/issues Michael T. Boulet Joshua Whitley + ament_cmake_auto diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 2e6f1bb..a7083ab 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -9,6 +9,7 @@ elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") message(STATUS "Changing CXX_STANDARD from C++98 to C++17") set(CMAKE_CXX_STANDARD 17) endif() +set(CMAKE_CXX_STANDARD 17) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 9928817..9e4c177 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -13,6 +13,7 @@ https://github.com/f1tenth/vesc/issues Michael T. Boulet Joshua Whitley + ament_cmake_auto From 5ea6c5f7664eedc1e7ac4cae232d300f035b0c40 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Fri, 6 Aug 2021 05:29:50 -1000 Subject: [PATCH 32/72] std::optional -> std::experimental::optional --- vesc/package.xml | 2 +- vesc_driver/CMakeLists.txt | 13 +++++----- .../include/vesc_driver/vesc_driver.hpp | 10 ++++---- vesc_driver/launch/vesc_driver_node.launch.py | 24 +++++++++++++++++++ vesc_driver/launch/vesc_node.launch.py | 0 vesc_driver/package.xml | 2 +- vesc_driver/src/vesc_driver.cpp | 4 ++-- 7 files changed, 39 insertions(+), 16 deletions(-) create mode 100644 vesc_driver/launch/vesc_driver_node.launch.py delete mode 100644 vesc_driver/launch/vesc_node.launch.py diff --git a/vesc/package.xml b/vesc/package.xml index ca4227b..e4748c1 100644 --- a/vesc/package.xml +++ b/vesc/package.xml @@ -13,7 +13,7 @@ https://github.com/f1tenth/vesc/issues Michael T. Boulet Joshua Whitley - + Haoru Xue ament_cmake_auto diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index a7083ab..368e0ca 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,15 +1,14 @@ cmake_minimum_required(VERSION 3.5) project(vesc_driver) -# Set minimum C++ standard to C++17 +# Set minimum C++ standard to C++14 if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++17") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++17") - set(CMAKE_CXX_STANDARD 17) + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_STANDARD 14) endif() -set(CMAKE_CXX_STANDARD 17) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() @@ -32,7 +31,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN vesc_driver::VescDriver - EXECUTABLE ${PROJECT_NAME}_node + EXECUTABLE vesc_node ) ############# diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index 2169fcb..f879f72 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include "vesc_driver/vesc_interface.hpp" #include "vesc_driver/vesc_packet.hpp" @@ -68,14 +68,14 @@ class VescDriver CommandLimit( rclcpp::Node * node_ptr, const std::string & str, - const std::optional & min_lower = std::optional(), - const std::optional & max_upper = std::optional()); + const std::experimental::optional & min_lower = std::experimental::optional(), + const std::experimental::optional & max_upper = std::experimental::optional()); double clip(double value); rclcpp::Node * node_ptr; rclcpp::Logger logger; std::string name; - std::optional lower; - std::optional upper; + std::experimental::optional lower; + std::experimental::optional upper; }; CommandLimit duty_cycle_limit_; diff --git a/vesc_driver/launch/vesc_driver_node.launch.py b/vesc_driver/launch/vesc_driver_node.launch.py new file mode 100644 index 0000000..2a82762 --- /dev/null +++ b/vesc_driver/launch/vesc_driver_node.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node +import os + + +def generate_launch_description(): + + vesc_config = os.path.join( + get_package_share_directory('vesc'), + 'params', + 'vesc_config.yaml' + ) + return LaunchDescription([ + Node( + package='vesc', + namespace='vesc', + executable='vesc_node', + name='vesc_node', + parameters= [vesc_config] + ), + + ]) + diff --git a/vesc_driver/launch/vesc_node.launch.py b/vesc_driver/launch/vesc_node.launch.py deleted file mode 100644 index e69de29..0000000 diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 9e4c177..ef527ed 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -13,7 +13,7 @@ https://github.com/f1tenth/vesc/issues Michael T. Boulet Joshua Whitley - + Haoru Xue ament_cmake_auto diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 6b92539..7295ac7 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -266,8 +266,8 @@ void VescDriver::servoCallback(const Float64::SharedPtr servo) VescDriver::CommandLimit::CommandLimit( rclcpp::Node * node_ptr, const std::string & str, - const std::optional & min_lower, - const std::optional & max_upper) + const std::experimental::optional & min_lower, + const std::experimental::optional & max_upper) : node_ptr(node_ptr), logger(node_ptr->get_logger()), name(str) From c2c9b8b86de164ee58b49e781b237a7b0aa3201f Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Fri, 6 Aug 2021 05:48:35 -1000 Subject: [PATCH 33/72] modified launch file --- vesc_driver/CMakeLists.txt | 2 +- vesc_driver/launch/vesc_driver_node.launch.py | 8 +++--- .../launch/vesc_driver_node.launch.xml | 28 ------------------- 3 files changed, 5 insertions(+), 33 deletions(-) delete mode 100644 vesc_driver/launch/vesc_driver_node.launch.xml diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 368e0ca..c6431ce 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -31,7 +31,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN vesc_driver::VescDriver - EXECUTABLE vesc_node + EXECUTABLE vesc_driver_node ) ############# diff --git a/vesc_driver/launch/vesc_driver_node.launch.py b/vesc_driver/launch/vesc_driver_node.launch.py index 2a82762..82ab171 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.py +++ b/vesc_driver/launch/vesc_driver_node.launch.py @@ -7,16 +7,16 @@ def generate_launch_description(): vesc_config = os.path.join( - get_package_share_directory('vesc'), + get_package_share_directory('vesc_driver'), 'params', 'vesc_config.yaml' ) return LaunchDescription([ Node( - package='vesc', + package='vesc_driver', namespace='vesc', - executable='vesc_node', - name='vesc_node', + executable='vesc_driver_node', + name='vesc_driver_node', parameters= [vesc_config] ), diff --git a/vesc_driver/launch/vesc_driver_node.launch.xml b/vesc_driver/launch/vesc_driver_node.launch.xml deleted file mode 100644 index 0e8a8ce..0000000 --- a/vesc_driver/launch/vesc_driver_node.launch.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 448675ef488051955677adc7a58956d091a458be Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Fri, 6 Aug 2021 08:21:07 -1000 Subject: [PATCH 34/72] update readme --- README.md | 24 +++++++++++++------ vesc_driver/launch/vesc_driver_node.launch.py | 1 - vesc_driver/src/vesc_interface.cpp | 2 +- 3 files changed, 18 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index ff49614..575fa6e 100644 --- a/README.md +++ b/README.md @@ -5,23 +5,33 @@ Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details # Development Log +Haoru Xue 8/6/2021 + +Completed: + +- Fall back to C++ 14 +- Enabled error handler + + Haoru Xue 5/16/2021 -## Completed +Completed: + - Ported the VESC package to ROS2 - Sensor reading tested - Speed and steerign commands tested -## Potential Improvements +Potential Improvements: + - C++ 17 is currently required to replace `boost::optional` with `std::optional` - Speed sensor reading is inverted - Not made LifeCycleNode yet ## How to test -1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src` folder. + +1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src`. 2. `rosdep update && rosdep install --from-paths src -i -y` 3. Plug in the VESC with a USB cable. -4. Check the device name, typically `/dev/ttyACM0`. -5. Modify `vesc/vesc_driver/params/vesc_config.yaml` to reflect any changes. -6. Build the package `colcon build --cmake-args '-DCMAKE_BUILD_TYPE=Debug' --packages-up-to vesc` -7. `ros2 run vesc_driver vesc_driver_node --ros-args -r __node:=vesc_node -r __ns:=/ --params-file /root/ros2_ws/src/vesc/vesc_driver/params/vesc_config.yaml` \ No newline at end of file +4. Modify `vesc/vesc_driver/params/vesc_config.yaml` to reflect any changes. +5. Build the package `colcon build` +6. `ros2 launch vesc_driver vesc_driver_node.launch.py` \ No newline at end of file diff --git a/vesc_driver/launch/vesc_driver_node.launch.py b/vesc_driver/launch/vesc_driver_node.launch.py index 82ab171..ffd83aa 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.py +++ b/vesc_driver/launch/vesc_driver_node.launch.py @@ -14,7 +14,6 @@ def generate_launch_description(): return LaunchDescription([ Node( package='vesc_driver', - namespace='vesc', executable='vesc_driver_node', name='vesc_driver_node', parameters= [vesc_config] diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 03463f3..bf5bdc8 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -156,7 +156,7 @@ void VescInterface::Impl::packet_creation_thread() void VescInterface::Impl::connect(const std::string & port) { uint32_t baud_rate = 115200; - auto fc = drivers::serial_driver::FlowControl::NONE; + auto fc = drivers::serial_driver::FlowControl::HARDWARE; auto pt = drivers::serial_driver::Parity::NONE; auto sb = drivers::serial_driver::StopBits::ONE; device_config_ = std::make_unique(baud_rate, fc, pt, sb); From 7fc6c0d238850b74f4f0ecd56da6ecce09ca6728 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Fri, 6 Aug 2021 09:21:16 -1000 Subject: [PATCH 35/72] removed old error handler --- vesc_driver/src/vesc_interface.cpp | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index bf5bdc8..6a6b947 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -107,13 +107,6 @@ void VescInterface::Impl::packet_creation_thread() VescPacketConstPtr packet = VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error); if (packet) { - // good packet, check if we skipped any data - //if (std::distance(iter_begin, iter) > 0) { - //std::ostringstream ss; - //ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << - // std::distance(iter_begin, iter) << " bytes."; - //error_handler_(ss.str()); - //} // call packet handler packet_handler_(packet); // update state @@ -124,9 +117,6 @@ void VescInterface::Impl::packet_creation_thread() } else if (bytes_needed > 0) { // need more data, break out of while loop break; // for (iter_sof... - } else { - // else, this was not a packet, move on to next byte - //error_handler_(error); } } @@ -139,11 +129,6 @@ void VescInterface::Impl::packet_creation_thread() } // erase "used" buffer - //if (std::distance(iter_begin, iter) > 0) { - //std::ostringstream ss; - //ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; - //error_handler_(ss.str()); - //} buffer_.erase(buffer_.begin(), iter); } From c47fccbbd10fb66db3faaaa6e469f2eedba2586f Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Sat, 22 May 2021 12:46:47 -0700 Subject: [PATCH 36/72] resolved inbound serial comunication problems --- vesc_driver/src/vesc_driver.cpp | 5 ++++- vesc_driver/src/vesc_interface.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 7295ac7..686300c 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,7 +65,10 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - std::string port = declare_parameter("port", "/dev/ttyACM0"); + // default to /tmp/ttyV0 for debug purpose + // use socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' + // to sniff comunication data to and from vesc + std::string port = declare_parameter("port", "/tmp/ttyV0"); // attempt to connect to the serial port try { diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 6a6b947..5e7a6f9 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -198,7 +198,7 @@ void VescInterface::connect(const std::string & port) impl_->connect(port); } catch (const std::exception & e) { std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); + ss << "Failed to open the serial port "<< port <<" to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } From 6fa37890ca3debf7ade2e0d3f312ea11011dc162 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Sat, 22 May 2021 13:31:52 -0700 Subject: [PATCH 37/72] protocol update to FW 5.2 --- vesc_driver/include/vesc_driver/datatypes.hpp | 1107 ++++++++++++++--- .../include/vesc_driver/vesc_packet.hpp | 62 +- vesc_driver/src/vesc_driver.cpp | 40 +- vesc_driver/src/vesc_packet.cpp | 275 +++- vesc_msgs/msg/VescState.msg | 37 +- 5 files changed, 1276 insertions(+), 245 deletions(-) diff --git a/vesc_driver/include/vesc_driver/datatypes.hpp b/vesc_driver/include/vesc_driver/datatypes.hpp index e195182..c3043e4 100644 --- a/vesc_driver/include/vesc_driver/datatypes.hpp +++ b/vesc_driver/include/vesc_driver/datatypes.hpp @@ -28,24 +28,29 @@ /* - Some parts of this code, Copyright 2016 - 2017 Benjamin Vedder benjamin@vedder.se + Some parts of this code, Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se - VESC Tool is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. + This file is part of the VESC firmware. - VESC Tool is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. -#ifndef VESC_DRIVER__DATATYPES_HPP_ -#define VESC_DRIVER__DATATYPES_HPP_ + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#ifndef DATATYPES_H_ +#define DATATYPES_H_ + +#include +#include #include @@ -81,139 +86,961 @@ typedef enum VESC_TX_DOUBLE16, VESC_TX_DOUBLE32, VESC_TX_DOUBLE32_AUTO -} -VESC_TX_T; +} VESC_TX_T; -typedef enum -{ - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR -} -mc_fault_code; +//#include "ch.h" +typedef uint32_t systime_t; // defined in ch.h -typedef enum -{ - DISP_POS_MODE_NONE = 0, - DISP_POS_MODE_INDUCTANCE, - DISP_POS_MODE_OBSERVER, - DISP_POS_MODE_ENCODER, - DISP_POS_MODE_PID_POS, - DISP_POS_MODE_PID_POS_ERROR, - DISP_POS_MODE_ENCODER_OBSERVER_ERROR -} -disp_pos_mode; +// Data types +typedef enum { + MC_STATE_OFF = 0, + MC_STATE_DETECTING, + MC_STATE_RUNNING, + MC_STATE_FULL_BRAKE, +} mc_state; -struct MC_VALUES -{ -public: - double v_in; - double temp_mos; - double temp_motor; - double current_motor; - double current_in; - double id; - double iq; - double rpm; - double duty_now; - double amp_hours; - double amp_hours_charged; - double watt_hours; - double watt_hours_charged; - int tachometer; - int tachometer_abs; - double position; - mc_fault_code fault_code; -}; +typedef enum { + PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended + PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode + PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs +} mc_pwm_mode; -typedef enum -{ - DEBUG_SAMPLING_OFF = 0, - DEBUG_SAMPLING_NOW, - DEBUG_SAMPLING_START, - DEBUG_SAMPLING_TRIGGER_START, - DEBUG_SAMPLING_TRIGGER_FAULT, - DEBUG_SAMPLING_TRIGGER_START_NOSEND, - DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, - DEBUG_SAMPLING_SEND_LAST_SAMPLES -} -debug_sampling_mode; +typedef enum { + COMM_MODE_INTEGRATE = 0, + COMM_MODE_DELAY +} mc_comm_mode; -typedef enum -{ - COMM_FW_VERSION = 0, - COMM_JUMP_TO_BOOTLOADER, - COMM_ERASE_NEW_APP, - COMM_WRITE_NEW_APP_DATA, - COMM_GET_VALUES, - COMM_SET_DUTY, - COMM_SET_CURRENT, - COMM_SET_CURRENT_BRAKE, - COMM_SET_RPM, - COMM_SET_POS, - COMM_SET_HANDBRAKE, - COMM_SET_DETECT, - COMM_SET_SERVO_POS, - COMM_SET_MCCONF, - COMM_GET_MCCONF, - COMM_GET_MCCONF_DEFAULT, - COMM_SET_APPCONF, - COMM_GET_APPCONF, - COMM_GET_APPCONF_DEFAULT, - COMM_SAMPLE_PRINT, - COMM_TERMINAL_CMD, - COMM_PRINT, - COMM_ROTOR_POSITION, - COMM_EXPERIMENT_SAMPLE, - COMM_DETECT_MOTOR_PARAM, - COMM_DETECT_MOTOR_R_L, - COMM_DETECT_MOTOR_FLUX_LINKAGE, - COMM_DETECT_ENCODER, - COMM_DETECT_HALL_FOC, - COMM_REBOOT, - COMM_ALIVE, - COMM_GET_DECODED_PPM, - COMM_GET_DECODED_ADC, - COMM_GET_DECODED_CHUK, - COMM_FORWARD_CAN, - COMM_SET_CHUCK_DATA, - COMM_CUSTOM_APP_DATA, - COMM_NRF_START_PAIRING -} -COMM_PACKET_ID; +typedef enum { + SENSOR_MODE_SENSORLESS = 0, + SENSOR_MODE_SENSORED, + SENSOR_MODE_HYBRID +} mc_sensor_mode; -typedef struct -{ - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; -} -chuck_data; +typedef enum { + FOC_SENSOR_MODE_SENSORLESS = 0, + FOC_SENSOR_MODE_ENCODER, + FOC_SENSOR_MODE_HALL, + FOC_SENSOR_MODE_HFI +} mc_foc_sensor_mode; -struct bldc_detect -{ -public: - double cycle_int_limit; - double bemf_coupling_k; - int hall_res; -}; +// Auxiliary output mode +typedef enum { + OUT_AUX_MODE_OFF = 0, + OUT_AUX_MODE_ON_AFTER_2S, + OUT_AUX_MODE_ON_AFTER_5S, + OUT_AUX_MODE_ON_AFTER_10S, + OUT_AUX_MODE_UNUSED +} out_aux_mode; -typedef enum -{ - NRF_PAIR_STARTED = 0, - NRF_PAIR_OK, - NRF_PAIR_FAIL -} -NRF_PAIR_RES; +// Temperature sensor type +typedef enum { + TEMP_SENSOR_NTC_10K_25C = 0, + TEMP_SENSOR_PTC_1K_100C, + TEMP_SENSOR_KTY83_122, +} temp_sensor_type; + +// General purpose drive output mode +typedef enum { + GPD_OUTPUT_MODE_NONE = 0, + GPD_OUTPUT_MODE_MODULATION, + GPD_OUTPUT_MODE_VOLTAGE, + GPD_OUTPUT_MODE_CURRENT +} gpd_output_mode; + +typedef enum { + MOTOR_TYPE_BLDC = 0, + MOTOR_TYPE_DC, + MOTOR_TYPE_FOC, + MOTOR_TYPE_GPD +} mc_motor_type; + +// FOC current controller decoupling mode. +typedef enum { + FOC_CC_DECOUPLING_DISABLED = 0, + FOC_CC_DECOUPLING_CROSS, + FOC_CC_DECOUPLING_BEMF, + FOC_CC_DECOUPLING_CROSS_BEMF +} mc_foc_cc_decoupling_mode; + +typedef enum { + FOC_OBSERVER_ORTEGA_ORIGINAL = 0, + FOC_OBSERVER_ORTEGA_ITERATIVE +} mc_foc_observer_type; + +typedef enum { + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR, + FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, + FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, + FAULT_CODE_MCU_UNDER_VOLTAGE, + FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, + FAULT_CODE_ENCODER_SPI, + FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, + FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, + FAULT_CODE_FLASH_CORRUPTION, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, + FAULT_CODE_UNBALANCED_CURRENTS, + FAULT_CODE_BRK, + FAULT_CODE_RESOLVER_LOT, + FAULT_CODE_RESOLVER_DOS, + FAULT_CODE_RESOLVER_LOS +} mc_fault_code; + +typedef enum { + CONTROL_MODE_DUTY = 0, + CONTROL_MODE_SPEED, + CONTROL_MODE_CURRENT, + CONTROL_MODE_CURRENT_BRAKE, + CONTROL_MODE_POS, + CONTROL_MODE_HANDBRAKE, + CONTROL_MODE_OPENLOOP, + CONTROL_MODE_OPENLOOP_PHASE, + CONTROL_MODE_OPENLOOP_DUTY, + CONTROL_MODE_OPENLOOP_DUTY_PHASE, + CONTROL_MODE_NONE +} mc_control_mode; + +typedef enum { + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR +} disp_pos_mode; + +typedef enum { + SENSOR_PORT_MODE_HALL = 0, + SENSOR_PORT_MODE_ABI, + SENSOR_PORT_MODE_AS5047_SPI, + SENSOR_PORT_MODE_AD2S1205, + SENSOR_PORT_MODE_SINCOS, + SENSOR_PORT_MODE_TS5700N8501, + SENSOR_PORT_MODE_TS5700N8501_MULTITURN +} sensor_port_mode; + +typedef struct { + float cycle_int_limit; + float cycle_int_limit_running; + float cycle_int_limit_max; + float comm_time_sum; + float comm_time_sum_min_rpm; + int32_t comms; + float time_at_comm; +} mc_rpm_dep_struct; + +typedef enum { + DRV8301_OC_LIMIT = 0, + DRV8301_OC_LATCH_SHUTDOWN, + DRV8301_OC_REPORT_ONLY, + DRV8301_OC_DISABLED +} drv8301_oc_mode; + +typedef enum { + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES +} debug_sampling_mode; + +typedef enum { + CAN_BAUD_125K = 0, + CAN_BAUD_250K, + CAN_BAUD_500K, + CAN_BAUD_1M, + CAN_BAUD_10K, + CAN_BAUD_20K, + CAN_BAUD_50K, + CAN_BAUD_75K +} CAN_BAUD; + +typedef enum { + BATTERY_TYPE_LIION_3_0__4_2, + BATTERY_TYPE_LIIRON_2_6__3_6, + BATTERY_TYPE_LEAD_ACID +} BATTERY_TYPE; + +typedef enum { + HFI_SAMPLES_8 = 0, + HFI_SAMPLES_16, + HFI_SAMPLES_32 +} FOC_HFI_SAMPLES; + +typedef struct { + // Switching and drive + mc_pwm_mode pwm_mode; + mc_comm_mode comm_mode; + mc_motor_type motor_type; + mc_sensor_mode sensor_mode; + // Limits + float l_current_max; + float l_current_min; + float l_in_current_max; + float l_in_current_min; + float l_abs_current_max; + float l_min_erpm; + float l_max_erpm; + float l_erpm_start; + float l_max_erpm_fbrake; + float l_max_erpm_fbrake_cc; + float l_min_vin; + float l_max_vin; + float l_battery_cut_start; + float l_battery_cut_end; + bool l_slow_abs_current; + float l_temp_fet_start; + float l_temp_fet_end; + float l_temp_motor_start; + float l_temp_motor_end; + float l_temp_accel_dec; + float l_min_duty; + float l_max_duty; + float l_watt_max; + float l_watt_min; + float l_current_max_scale; + float l_current_min_scale; + float l_duty_start; + // Overridden limits (Computed during runtime) + float lo_current_max; + float lo_current_min; + float lo_in_current_max; + float lo_in_current_min; + float lo_current_motor_max_now; + float lo_current_motor_min_now; + // Sensorless (bldc) + float sl_min_erpm; + float sl_min_erpm_cycle_int_limit; + float sl_max_fullbreak_current_dir_change; + float sl_cycle_int_limit; + float sl_phase_advance_at_br; + float sl_cycle_int_rpm_br; + float sl_bemf_coupling_k; + // Hall sensor + int8_t hall_table[8]; + float hall_sl_erpm; + // FOC + float foc_current_kp; + float foc_current_ki; + float foc_f_sw; + float foc_dt_us; + float foc_encoder_offset; + bool foc_encoder_inverted; + float foc_encoder_ratio; + float foc_encoder_sin_offset; + float foc_encoder_sin_gain; + float foc_encoder_cos_offset; + float foc_encoder_cos_gain; + float foc_encoder_sincos_filter_constant; + float foc_motor_l; + float foc_motor_r; + float foc_motor_flux_linkage; + float foc_observer_gain; + float foc_observer_gain_slow; + float foc_pll_kp; + float foc_pll_ki; + float foc_duty_dowmramp_kp; + float foc_duty_dowmramp_ki; + float foc_openloop_rpm; + float foc_sl_openloop_hyst; + float foc_sl_openloop_time; + float foc_sl_d_current_duty; + float foc_sl_d_current_factor; + mc_foc_sensor_mode foc_sensor_mode; + uint8_t foc_hall_table[8]; + float foc_sl_erpm; + bool foc_sample_v0_v7; + bool foc_sample_high_current; + float foc_sat_comp; + bool foc_temp_comp; + float foc_temp_comp_base_temp; + float foc_current_filter_const; + mc_foc_cc_decoupling_mode foc_cc_decoupling; + mc_foc_observer_type foc_observer_type; + float foc_hfi_voltage_start; + float foc_hfi_voltage_run; + float foc_hfi_voltage_max; + float foc_sl_erpm_hfi; + uint16_t foc_hfi_start_samples; + float foc_hfi_obs_ovr_sec; + FOC_HFI_SAMPLES foc_hfi_samples; + // GPDrive + int gpd_buffer_notify_left; + int gpd_buffer_interpol; + float gpd_current_filter_const; + float gpd_current_kp; + float gpd_current_ki; + // Speed PID + float s_pid_kp; + float s_pid_ki; + float s_pid_kd; + float s_pid_kd_filter; + float s_pid_min_erpm; + bool s_pid_allow_braking; + // Pos PID + float p_pid_kp; + float p_pid_ki; + float p_pid_kd; + float p_pid_kd_filter; + float p_pid_ang_div; + // Current controller + float cc_startup_boost_duty; + float cc_min_current; + float cc_gain; + float cc_ramp_step_max; + // Misc + int32_t m_fault_stop_time_ms; + float m_duty_ramp_step; + float m_current_backoff_gain; + uint32_t m_encoder_counts; + sensor_port_mode m_sensor_port_mode; + bool m_invert_direction; + drv8301_oc_mode m_drv8301_oc_mode; + int m_drv8301_oc_adj; + float m_bldc_f_sw_min; + float m_bldc_f_sw_max; + float m_dc_f_sw; + float m_ntc_motor_beta; + out_aux_mode m_out_aux_mode; + temp_sensor_type m_motor_temp_sens_type; + float m_ptc_motor_coeff; + // Setup info + uint8_t si_motor_poles; + float si_gear_ratio; + float si_wheel_diameter; + BATTERY_TYPE si_battery_type; + int si_battery_cells; + float si_battery_ah; +} mc_configuration; + +// Applications to use +typedef enum { + APP_NONE = 0, + APP_PPM, + APP_ADC, + APP_UART, + APP_PPM_UART, + APP_ADC_UART, + APP_NUNCHUK, + APP_NRF, + APP_CUSTOM, + APP_BALANCE +} app_use; + +// Throttle curve mode +typedef enum { + THR_EXP_EXPO = 0, + THR_EXP_NATURAL, + THR_EXP_POLY +} thr_exp_mode; + +// PPM control types +typedef enum { + PPM_CTRL_TYPE_NONE = 0, + PPM_CTRL_TYPE_CURRENT, + PPM_CTRL_TYPE_CURRENT_NOREV, + PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, + PPM_CTRL_TYPE_DUTY, + PPM_CTRL_TYPE_DUTY_NOREV, + PPM_CTRL_TYPE_PID, + PPM_CTRL_TYPE_PID_NOREV, + PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST, + PPM_CTRL_TYPE_CURRENT_SMART_REV +} ppm_control_type; + +typedef struct { + ppm_control_type ctrl_type; + float pid_max_erpm; + float hyst; + float pulse_start; + float pulse_end; + float pulse_center; + bool median_filter; + bool safe_start; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + float max_erpm_for_dir; + float smart_rev_max_duty; + float smart_rev_ramp_time; +} ppm_config; + +// ADC control types +typedef enum { + ADC_CTRL_TYPE_NONE = 0, + ADC_CTRL_TYPE_CURRENT, + ADC_CTRL_TYPE_CURRENT_REV_CENTER, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, + ADC_CTRL_TYPE_DUTY, + ADC_CTRL_TYPE_DUTY_REV_CENTER, + ADC_CTRL_TYPE_DUTY_REV_BUTTON, + ADC_CTRL_TYPE_PID, + ADC_CTRL_TYPE_PID_REV_CENTER, + ADC_CTRL_TYPE_PID_REV_BUTTON +} adc_control_type; -} // namespace vesc_driver +typedef struct { + adc_control_type ctrl_type; + float hyst; + float voltage_start; + float voltage_end; + float voltage_center; + float voltage2_start; + float voltage2_end; + bool use_filter; + bool safe_start; + bool cc_button_inverted; + bool rev_button_inverted; + bool voltage_inverted; + bool voltage2_inverted; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + uint32_t update_rate_hz; +} adc_config; -#endif // VESC_DRIVER__DATATYPES_HPP_ +// Nunchuk control types +typedef enum { + CHUK_CTRL_TYPE_NONE = 0, + CHUK_CTRL_TYPE_CURRENT, + CHUK_CTRL_TYPE_CURRENT_NOREV +} chuk_control_type; + +typedef struct { + chuk_control_type ctrl_type; + float hyst; + float ramp_time_pos; + float ramp_time_neg; + float stick_erpm_per_s_in_cc; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + bool multi_esc; + bool tc; + float tc_max_diff; + bool use_smart_rev; + float smart_rev_max_duty; + float smart_rev_ramp_time; +} chuk_config; + +// NRF Datatypes +typedef enum { + NRF_SPEED_250K = 0, + NRF_SPEED_1M, + NRF_SPEED_2M +} NRF_SPEED; + +typedef enum { + NRF_POWER_M18DBM = 0, + NRF_POWER_M12DBM, + NRF_POWER_M6DBM, + NRF_POWER_0DBM, + NRF_POWER_OFF +} NRF_POWER; + +typedef enum { + NRF_AW_3 = 0, + NRF_AW_4, + NRF_AW_5 +} NRF_AW; + +typedef enum { + NRF_CRC_DISABLED = 0, + NRF_CRC_1B, + NRF_CRC_2B +} NRF_CRC; + +typedef enum { + NRF_RETR_DELAY_250US = 0, + NRF_RETR_DELAY_500US, + NRF_RETR_DELAY_750US, + NRF_RETR_DELAY_1000US, + NRF_RETR_DELAY_1250US, + NRF_RETR_DELAY_1500US, + NRF_RETR_DELAY_1750US, + NRF_RETR_DELAY_2000US, + NRF_RETR_DELAY_2250US, + NRF_RETR_DELAY_2500US, + NRF_RETR_DELAY_2750US, + NRF_RETR_DELAY_3000US, + NRF_RETR_DELAY_3250US, + NRF_RETR_DELAY_3500US, + NRF_RETR_DELAY_3750US, + NRF_RETR_DELAY_4000US +} NRF_RETR_DELAY; + +typedef struct { + NRF_SPEED speed; + NRF_POWER power; + NRF_CRC crc_type; + NRF_RETR_DELAY retry_delay; + unsigned char retries; + unsigned char channel; + unsigned char address[3]; + bool send_crc_ack; +} nrf_config; + +typedef struct { + float kp; + float ki; + float kd; + uint16_t hertz; + float pitch_fault; + float roll_fault; + float adc1; + float adc2; + float overspeed_duty; + float tiltback_duty; + float tiltback_angle; + float tiltback_speed; + float tiltback_high_voltage; + float tiltback_low_voltage; + float startup_pitch_tolerance; + float startup_roll_tolerance; + float startup_speed; + float deadzone; + float current_boost; + bool multi_esc; + float yaw_kp; + float yaw_ki; + float yaw_kd; + float roll_steer_kp; + float brake_current; + uint16_t overspeed_delay; + uint16_t fault_delay; + float tiltback_constant; + float roll_steer_erpm_kp; + float yaw_current_clamp; + uint16_t adc_half_fault_erpm; + float setpoint_pitch_filter; + float setpoint_target_filter; + float setpoint_clamp; +} balance_config; + +// CAN status modes +typedef enum { + CAN_STATUS_DISABLED = 0, + CAN_STATUS_1, + CAN_STATUS_1_2, + CAN_STATUS_1_2_3, + CAN_STATUS_1_2_3_4, + CAN_STATUS_1_2_3_4_5 +} CAN_STATUS_MODE; + +typedef enum { + SHUTDOWN_MODE_ALWAYS_OFF = 0, + SHUTDOWN_MODE_ALWAYS_ON, + SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY, + SHUTDOWN_MODE_OFF_AFTER_10S, + SHUTDOWN_MODE_OFF_AFTER_1M, + SHUTDOWN_MODE_OFF_AFTER_5M, + SHUTDOWN_MODE_OFF_AFTER_10M, + SHUTDOWN_MODE_OFF_AFTER_30M, + SHUTDOWN_MODE_OFF_AFTER_1H, + SHUTDOWN_MODE_OFF_AFTER_5H, +} SHUTDOWN_MODE; + +typedef enum { + IMU_TYPE_OFF = 0, + IMU_TYPE_INTERNAL, + IMU_TYPE_EXTERNAL_MPU9X50, + IMU_TYPE_EXTERNAL_ICM20948, + IMU_TYPE_EXTERNAL_BMI160 +} IMU_TYPE; + +typedef enum { + AHRS_MODE_MADGWICK = 0, + AHRS_MODE_MAHONY +} AHRS_MODE; + +typedef struct { + IMU_TYPE type; + AHRS_MODE mode; + int sample_rate_hz; + float accel_confidence_decay; + float mahony_kp; + float mahony_ki; + float madgwick_beta; + float rot_roll; + float rot_pitch; + float rot_yaw; + float accel_offsets[3]; + float gyro_offsets[3]; + float gyro_offset_comp_fact[3]; + float gyro_offset_comp_clamp; +} imu_config; + +typedef enum { + CAN_MODE_VESC = 0, + CAN_MODE_UAVCAN, + CAN_MODE_COMM_BRIDGE +} CAN_MODE; + +typedef struct { + // Settings + uint8_t controller_id; + uint32_t timeout_msec; + float timeout_brake_current; + CAN_STATUS_MODE send_can_status; + uint32_t send_can_status_rate_hz; + CAN_BAUD can_baud_rate; + bool pairing_done; + bool permanent_uart_enabled; + SHUTDOWN_MODE shutdown_mode; + + // CAN modes + CAN_MODE can_mode; + uint8_t uavcan_esc_index; + + // Application to use + app_use app_to_use; + + // PPM application settings + ppm_config app_ppm_conf; + + // ADC application settings + adc_config app_adc_conf; + + // UART application settings + uint32_t app_uart_baudrate; + + // Nunchuk application settings + chuk_config app_chuk_conf; + + // NRF application settings + nrf_config app_nrf_conf; + + // Balance application settings + balance_config app_balance_conf; + + // IMU Settings + imu_config imu_conf; +} app_configuration; + +// Communication commands +typedef enum { + COMM_FW_VERSION = 0, + COMM_JUMP_TO_BOOTLOADER, + COMM_ERASE_NEW_APP, + COMM_WRITE_NEW_APP_DATA, + COMM_GET_VALUES, + COMM_SET_DUTY, + COMM_SET_CURRENT, + COMM_SET_CURRENT_BRAKE, + COMM_SET_RPM, + COMM_SET_POS, + COMM_SET_HANDBRAKE, + COMM_SET_DETECT, + COMM_SET_SERVO_POS, + COMM_SET_MCCONF, + COMM_GET_MCCONF, + COMM_GET_MCCONF_DEFAULT, + COMM_SET_APPCONF, + COMM_GET_APPCONF, + COMM_GET_APPCONF_DEFAULT, + COMM_SAMPLE_PRINT, + COMM_TERMINAL_CMD, + COMM_PRINT, + COMM_ROTOR_POSITION, + COMM_EXPERIMENT_SAMPLE, + COMM_DETECT_MOTOR_PARAM, + COMM_DETECT_MOTOR_R_L, + COMM_DETECT_MOTOR_FLUX_LINKAGE, + COMM_DETECT_ENCODER, + COMM_DETECT_HALL_FOC, + COMM_REBOOT, + COMM_ALIVE, + COMM_GET_DECODED_PPM, + COMM_GET_DECODED_ADC, + COMM_GET_DECODED_CHUK, + COMM_FORWARD_CAN, + COMM_SET_CHUCK_DATA, + COMM_CUSTOM_APP_DATA, + COMM_NRF_START_PAIRING, + COMM_GPD_SET_FSW, + COMM_GPD_BUFFER_NOTIFY, + COMM_GPD_BUFFER_SIZE_LEFT, + COMM_GPD_FILL_BUFFER, + COMM_GPD_OUTPUT_SAMPLE, + COMM_GPD_SET_MODE, + COMM_GPD_FILL_BUFFER_INT8, + COMM_GPD_FILL_BUFFER_INT16, + COMM_GPD_SET_BUFFER_INT_SCALE, + COMM_GET_VALUES_SETUP, + COMM_SET_MCCONF_TEMP, + COMM_SET_MCCONF_TEMP_SETUP, + COMM_GET_VALUES_SELECTIVE, + COMM_GET_VALUES_SETUP_SELECTIVE, + COMM_EXT_NRF_PRESENT, + COMM_EXT_NRF_ESB_SET_CH_ADDR, + COMM_EXT_NRF_ESB_SEND_DATA, + COMM_EXT_NRF_ESB_RX_DATA, + COMM_EXT_NRF_SET_ENABLED, + COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + COMM_DETECT_APPLY_ALL_FOC, + COMM_JUMP_TO_BOOTLOADER_ALL_CAN, + COMM_ERASE_NEW_APP_ALL_CAN, + COMM_WRITE_NEW_APP_DATA_ALL_CAN, + COMM_PING_CAN, + COMM_APP_DISABLE_OUTPUT, + COMM_TERMINAL_CMD_SYNC, + COMM_GET_IMU_DATA, + COMM_BM_CONNECT, + COMM_BM_ERASE_FLASH_ALL, + COMM_BM_WRITE_FLASH, + COMM_BM_REBOOT, + COMM_BM_DISCONNECT, + COMM_BM_MAP_PINS_DEFAULT, + COMM_BM_MAP_PINS_NRF5X, + COMM_ERASE_BOOTLOADER, + COMM_ERASE_BOOTLOADER_ALL_CAN, + COMM_PLOT_INIT, + COMM_PLOT_DATA, + COMM_PLOT_ADD_GRAPH, + COMM_PLOT_SET_GRAPH, + COMM_GET_DECODED_BALANCE, + COMM_BM_MEM_READ, + COMM_WRITE_NEW_APP_DATA_LZO, + COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, + COMM_BM_WRITE_FLASH_LZO, + COMM_SET_CURRENT_REL, + COMM_CAN_FWD_FRAME, + COMM_SET_BATTERY_CUT, + COMM_SET_BLE_NAME, + COMM_SET_BLE_PIN, + COMM_SET_CAN_MODE, + COMM_GET_IMU_CALIBRATION +} COMM_PACKET_ID; + +// CAN commands +typedef enum { + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT, + CAN_PACKET_SET_CURRENT_BRAKE, + CAN_PACKET_SET_RPM, + CAN_PACKET_SET_POS, + CAN_PACKET_FILL_RX_BUFFER, + CAN_PACKET_FILL_RX_BUFFER_LONG, + CAN_PACKET_PROCESS_RX_BUFFER, + CAN_PACKET_PROCESS_SHORT_BUFFER, + CAN_PACKET_STATUS, + CAN_PACKET_SET_CURRENT_REL, + CAN_PACKET_SET_CURRENT_BRAKE_REL, + CAN_PACKET_SET_CURRENT_HANDBRAKE, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, + CAN_PACKET_STATUS_2, + CAN_PACKET_STATUS_3, + CAN_PACKET_STATUS_4, + CAN_PACKET_PING, + CAN_PACKET_PONG, + CAN_PACKET_DETECT_APPLY_ALL_FOC, + CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, + CAN_PACKET_CONF_CURRENT_LIMITS, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS, + CAN_PACKET_CONF_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_FOC_ERPMS, + CAN_PACKET_CONF_STORE_FOC_ERPMS, + CAN_PACKET_STATUS_5, + CAN_PACKET_POLL_TS5700N8501_STATUS, + CAN_PACKET_CONF_BATTERY_CUT, + CAN_PACKET_CONF_STORE_BATTERY_CUT, + CAN_PACKET_SHUTDOWN +} CAN_PACKET_ID; + +// Logged fault data +typedef struct { + uint8_t motor; + mc_fault_code fault; + float current; + float current_filtered; + float voltage; + float gate_driver_voltage; + float duty; + float rpm; + int tacho; + int cycles_running; + int tim_val_samp; + int tim_current_samp; + int tim_top; + int comm_step; + float temperature; + int drv8301_faults; +} fault_data; + +// External LED state +typedef enum { + LED_EXT_OFF = 0, + LED_EXT_NORMAL, + LED_EXT_BRAKE, + LED_EXT_TURN_LEFT, + LED_EXT_TURN_RIGHT, + LED_EXT_BRAKE_TURN_LEFT, + LED_EXT_BRAKE_TURN_RIGHT, + LED_EXT_BATT +} LED_EXT_STATE; + +typedef struct { + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; + bool rev_has_state; + bool is_rev; +} chuck_data; + +typedef struct { + int id; + systime_t rx_time; + float rpm; + float current; + float duty; +} can_status_msg; + +typedef struct { + int id; + systime_t rx_time; + float amp_hours; + float amp_hours_charged; +} can_status_msg_2; + +typedef struct { + int id; + systime_t rx_time; + float watt_hours; + float watt_hours_charged; +} can_status_msg_3; + +typedef struct { + int id; + systime_t rx_time; + float temp_fet; + float temp_motor; + float current_in; + float pid_pos_now; +} can_status_msg_4; + +typedef struct { + int id; + systime_t rx_time; + float v_in; + int32_t tacho_value; +} can_status_msg_5; + +typedef struct { + uint8_t js_x; + uint8_t js_y; + bool bt_c; + bool bt_z; + bool bt_push; + bool rev_has_state; + bool is_rev; + float vbat; +} mote_state; + +typedef enum { + MOTE_PACKET_BATT_LEVEL = 0, + MOTE_PACKET_BUTTONS, + MOTE_PACKET_ALIVE, + MOTE_PACKET_FILL_RX_BUFFER, + MOTE_PACKET_FILL_RX_BUFFER_LONG, + MOTE_PACKET_PROCESS_RX_BUFFER, + MOTE_PACKET_PROCESS_SHORT_BUFFER, + MOTE_PACKET_PAIRING_INFO +} MOTE_PACKET; + +typedef struct { + float v_in; + float temp_mos; + float temp_mos_1; + float temp_mos_2; + float temp_mos_3; + float temp_motor; + float current_motor; + float current_in; + float id; + float iq; + float rpm; + float duty_now; + float amp_hours; + float amp_hours_charged; + float watt_hours; + float watt_hours_charged; + int tachometer; + int tachometer_abs; + float position; + mc_fault_code fault_code; + int vesc_id; + float vd; + float vq; +} mc_values; + +typedef enum { + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL +} NRF_PAIR_RES; + +// Orientation data +typedef struct { + float q0; + float q1; + float q2; + float q3; + float integralFBx; + float integralFBy; + float integralFBz; + float accMagP; + int initialUpdateDone; +} ATTITUDE_INFO; + +// Custom EEPROM variables +typedef union { + uint32_t as_u32; + int32_t as_i32; + float as_float; +} eeprom_var; + +#define EEPROM_VARS_HW 64 +#define EEPROM_VARS_CUSTOM 64 + +typedef struct { + float ah_tot; + float ah_charge_tot; + float wh_tot; + float wh_charge_tot; + float current_tot; + float current_in_tot; + uint8_t num_vescs; +} setup_values; +} +#endif /* DATATYPES_H_ */ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index a1e4dbd..fa1fc3c 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -120,6 +120,19 @@ class VescPacketFWVersion : public VescPacket int fwMajor() const; int fwMinor() const; + + std::string hwname() const; + const uint8_t* uuid() const; + bool paired() const; + uint8_t devVersion() const; + + private: + int minor_; + int major_; + std::string hwname_; + bool paired_; + uint8_t uuid_[12]; + uint8_t devVersion_; }; class VescPacketRequestFWVersion : public VescPacket @@ -135,25 +148,35 @@ class VescPacketValues : public VescPacket public: explicit VescPacketValues(std::shared_ptr raw); - double v_in() const; - double temp_mos1() const; - double temp_mos2() const; - double temp_mos3() const; - double temp_mos4() const; - double temp_mos5() const; - double temp_mos6() const; - double temp_pcb() const; - double current_motor() const; - double current_in() const; - double rpm() const; - double duty_now() const; - double amp_hours() const; - double amp_hours_charged() const; - double watt_hours() const; - double watt_hours_charged() const; - double tachometer() const; - double tachometer_abs() const; - int fault_code() const; +double temp_fet() const; +double temp_motor() const; +double avg_motor_current() const; +double avg_input_current() const; +double avg_id() const; +double avg_iq() const ; +double duty_cycle_now() const; +double rpm() const; +double duty_now() const; +double v_in() const; +double amp_hours() const; +double amp_hours_charged() const; +double watt_hours() const; +double watt_hours_charged() const; +int32_t tachometer() const; +int32_t tachometer_abs() const; +int fault_code() const; +double pid_pos_now() const; +int32_t controller_id() const; + +double temp_mos1() const; +double temp_mos2() const; +double temp_mos3() const; +double avg_vd() const; +double avg_vq() const; + +int32_t numVescs() const; +double watt_battery_left() const; + }; class VescPacketRequestValues : public VescPacket @@ -161,7 +184,6 @@ class VescPacketRequestValues : public VescPacket public: VescPacketRequestValues(); }; - /*------------------------------------------------------------------------------------------------*/ class VescPacketSetDuty : public VescPacket diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 686300c..1a237d4 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -160,28 +160,54 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa auto state_msg = VescStateStamped(); state_msg.header.stamp = now(); + state_msg.state.voltage_input = values->v_in(); - state_msg.state.temperature_pcb = values->temp_pcb(); - state_msg.state.current_motor = values->current_motor(); - state_msg.state.current_input = values->current_in(); - state_msg.state.speed = values->rpm(); - state_msg.state.duty_cycle = values->duty_now(); + state_msg.state.current_motor = values->avg_motor_current(); + state_msg.state.current_input = values->avg_input_current(); + state_msg.state.avg_id = values->avg_id(); + state_msg.state.avg_iq = values->avg_iq(); + state_msg.state.duty_cycle = values->duty_cycle_now(); + state_msg.state.speed = values->rpm(); + state_msg.state.charge_drawn = values->amp_hours(); state_msg.state.charge_regen = values->amp_hours_charged(); state_msg.state.energy_drawn = values->watt_hours(); state_msg.state.energy_regen = values->watt_hours_charged(); state_msg.state.displacement = values->tachometer(); state_msg.state.distance_traveled = values->tachometer_abs(); - state_msg.state.fault_code = values->fault_code(); + state_msg.state.fault_code = values->fault_code(); + + state_msg.state.pid_pos_now = values->pid_pos_now(); + state_msg.state.controller_id = values->controller_id(); + + state_msg.state.ntc_temp_mos1 = values->temp_mos1(); + state_msg.state.ntc_temp_mos2 = values->temp_mos2(); + state_msg.state.ntc_temp_mos3 = values->temp_mos3(); + state_msg.state.avg_vd = values->avg_vd(); + state_msg.state.avg_vq = values->avg_vq(); state_pub_->publish(state_msg); } else if (packet->name() == "FWVersion") { - std::shared_ptr fw_version = + std::shared_ptr fw_version = std::dynamic_pointer_cast(packet); // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); + + RCLCPP_INFO( + get_logger(), + "-=%s=- hardware paired %d", + fw_version->hwname().c_str(), + fw_version->paired() + ); } + + auto& clk = *this->get_clock(); + RCLCPP_INFO_THROTTLE( + get_logger(), + clk, + 5000, + "%s packet received",packet->name().c_str()); } void VescDriver::vescErrorCallback(const std::string & error) diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 5bb8df7..7ebf0cb 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -37,6 +37,7 @@ #include "vesc_driver/datatypes.hpp" #include "vesc_driver/vesc_packet_factory.hpp" +#include namespace vesc_driver { @@ -100,16 +101,59 @@ VescPacket::VescPacket(const std::string & name, std::shared_ptr raw) VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : VescPacket("FWVersion", raw) { + + major_ = *(payload_.first + 1); + minor_ = *(payload_.first + 2); + int j=0; + + + for(int i=0;*(payload_.first+3+i)!=0x00;i++){ + j = i; + hwname_ += *(payload_.first+3+i); + } + + j++; + + + + + for(int u=0;u<12;u++){ + uuid_[u] = *(payload_.first+3+j+1+u); + + } + + + paired_ = *(payload_.first+3+j+1+12); + devVersion_ = *(payload_.first+3+j+2+12+1); + } int VescPacketFWVersion::fwMajor() const { - return *(payload_.first + 1); + return major_; } int VescPacketFWVersion::fwMinor() const { - return *(payload_.first + 2); + return minor_; +} + +std::string VescPacketFWVersion::hwname() const +{ + + return hwname_.c_str(); +} +const uint8_t* VescPacketFWVersion::uuid() const +{ + return uuid_; +} +bool VescPacketFWVersion::paired() const +{ + return paired_; +} +uint8_t VescPacketFWVersion::devVersion() const +{ + return devVersion_; } REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) @@ -129,121 +173,216 @@ VescPacketValues::VescPacketValues(std::shared_ptr raw) : VescPacket("Values", raw) { } - -double VescPacketValues::temp_mos1() const -{ +double VescPacketValues::temp_fet() const{ int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); + static_cast(*(payload_.first + 2))); return static_cast(v) / 10.0; } -double VescPacketValues::temp_mos2() const -{ +double VescPacketValues::temp_motor() const{ int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - static_cast(*(payload_.first + 4))); + static_cast(*(payload_.first + 4))); 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))); + +double VescPacketValues::avg_motor_current() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + + static_cast(*(payload_.first + 8)) + ); 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))); + +double VescPacketValues::avg_input_current() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + static_cast(*(payload_.first + 12)) + ); return static_cast(v) / 100.0; + } -double VescPacketValues::duty_now() const +double VescPacketValues::avg_id() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 13)) << 24) + + (static_cast(*(payload_.first + 14)) << 16) + + (static_cast(*(payload_.first + 15)) << 8) + + static_cast(*(payload_.first + 16)) + ); + return static_cast(v) / 100.0; + +} + +double VescPacketValues::avg_iq() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 17)) << 24) + + (static_cast(*(payload_.first + 18)) << 16) + + (static_cast(*(payload_.first + 19)) << 8) + + static_cast(*(payload_.first + 20)) + ); + return static_cast(v) / 100.0; + +} + + +double VescPacketValues::duty_cycle_now() const { int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); + static_cast(*(payload_.first + 22))); return static_cast(v) / 1000.0; } -double VescPacketValues::rpm() const -{ - int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26))); - return static_cast(-1 * v); +double VescPacketValues::rpm() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + + static_cast(*(payload_.first + 26)) + ); + return static_cast(v) / 1.0; + } -double VescPacketValues::amp_hours() const +double VescPacketValues::v_in() const { - int32_t v = static_cast((static_cast(*(payload_.first + 27)) << 24) + - (static_cast(*(payload_.first + 28)) << 16) + - (static_cast(*(payload_.first + 29)) << 8) + - static_cast(*(payload_.first + 30))); - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 27)) << 8) + + static_cast(*(payload_.first + 28))); + return static_cast(v) /10.0; } -double VescPacketValues::amp_hours_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))); - return static_cast(v); +double VescPacketValues::amp_hours() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 29)) << 24) + + (static_cast(*(payload_.first + 30)) << 16) + + (static_cast(*(payload_.first + 31)) << 8) + + static_cast(*(payload_.first + 32)) + ); + return static_cast(v) / 1e4; + } -double VescPacketValues::tachometer() const +double VescPacketValues::amp_hours_charged() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 33)) << 24) + + (static_cast(*(payload_.first + 34)) << 16) + + (static_cast(*(payload_.first + 35)) << 8) + + static_cast(*(payload_.first + 36)) + ); + return static_cast(v) / 1e4; + +} + +double VescPacketValues::watt_hours() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 37)) << 24) + + (static_cast(*(payload_.first + 38)) << 16) + + (static_cast(*(payload_.first + 39)) << 8) + + static_cast(*(payload_.first + 40)) + ); + return static_cast(v) / 1e4; + +} + +double VescPacketValues::watt_hours_charged() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 41)) << 24) + + (static_cast(*(payload_.first + 42)) << 16) + + (static_cast(*(payload_.first + 43)) << 8) + + static_cast(*(payload_.first + 44)) + ); + return static_cast(v) / 1e4; + +} + +int32_t 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))); - return static_cast(v); + int32_t v = static_cast((static_cast(*(payload_.first + 45)) << 24) + + (static_cast(*(payload_.first + 46)) << 16) + + (static_cast(*(payload_.first + 47)) << 8) + + static_cast(*(payload_.first + 48))); + return static_cast(v); } -double VescPacketValues::tachometer_abs() const +int32_t 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))); - return static_cast(v); + int32_t v = static_cast((static_cast(*(payload_.first + 49)) << 24) + + (static_cast(*(payload_.first + 50)) << 16) + + (static_cast(*(payload_.first + 51)) << 8) + + static_cast(*(payload_.first + 52))); + return static_cast(v); } int VescPacketValues::fault_code() const { - return static_cast(*(payload_.first + 56)); + return static_cast(*(payload_.first + 53)); } -double VescPacketValues::v_in() const + +double VescPacketValues::pid_pos_now() const +{ + int32_t v = static_cast((static_cast(*(payload_.first + 54)) << 24) + + (static_cast(*(payload_.first + 55)) << 16) + + (static_cast(*(payload_.first + 56)) << 8) + + static_cast(*(payload_.first + 57)) + ); + return static_cast(v) / 1e6; + +} + +int32_t VescPacketValues::controller_id() const { - int32_t v = 0; - return static_cast(v); + //int32_t v = static_cast((static_cast(*(payload_.first + 58)) << 24) + + // (static_cast(*(payload_.first + 59)) << 16) + + // (static_cast(*(payload_.first + 60)) << 8) + + // static_cast(*(payload_.first + 61))); +// + int32_t v = static_cast((static_cast(*(payload_.first + 58) ))); + + return static_cast(v); } -double VescPacketValues::temp_pcb() const +double VescPacketValues::temp_mos1() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 59)) << 8) + + static_cast(*(payload_.first + 60))); + return static_cast(v) / 10.0; } -double VescPacketValues::watt_hours() const +double VescPacketValues::temp_mos2() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 61)) << 8) + + static_cast(*(payload_.first + 62))); + return static_cast(v) / 10.0; } -double VescPacketValues::watt_hours_charged() const +double VescPacketValues::temp_mos3() const { - int32_t v = 0; - return static_cast(v); + int16_t v = static_cast((static_cast(*(payload_.first + 63)) << 8) + + static_cast(*(payload_.first + 64))); + return static_cast(v) / 10.0; } +double VescPacketValues::avg_vd() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 65)) << 24) + + (static_cast(*(payload_.first + 66)) << 16) + + (static_cast(*(payload_.first + 67)) << 8) + + static_cast(*(payload_.first + 68)) + ); + return static_cast(v) / 1e3; + +} + + +double VescPacketValues::avg_vq() const{ + int32_t v = static_cast((static_cast(*(payload_.first + 69)) << 24) + + (static_cast(*(payload_.first + 70)) << 16) + + (static_cast(*(payload_.first + 71)) << 8) + + static_cast(*(payload_.first + 72)) + ); + return static_cast(v) / 1e3; + +} + + REGISTER_PACKET_TYPE(COMM_GET_VALUES, VescPacketValues) VescPacketRequestValues::VescPacketRequestValues() diff --git a/vesc_msgs/msg/VescState.msg b/vesc_msgs/msg/VescState.msg index 276d62a..2edfd30 100644 --- a/vesc_msgs/msg/VescState.msg +++ b/vesc_msgs/msg/VescState.msg @@ -9,16 +9,33 @@ int32 FAULT_CODE_ABS_OVER_CURRENT=4 int32 FAULT_CODE_OVER_TEMP_FET=5 int32 FAULT_CODE_OVER_TEMP_MOTOR=6 +# follow the bledc firwmare: commands.c +float64 temp_fet # fet temperature +float64 temp_motor # motor temperature +float64 current_motor # motor current (ampere) avg_motor_current +float64 current_input # input current (ampere) avg_input_current +float64 avg_id +float64 avg_iq +float64 duty_cycle # duty cycle (0 to 1) duty_cycle_now +float64 speed # motor electrical speed (revolutions per minute) rpm + float64 voltage_input # input voltage (volt) -float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) -float64 current_motor # motor current (ampere) -float64 current_input # input current (ampere) -float64 speed # motor electrical speed (revolutions per minute) -float64 duty_cycle # duty cycle (0 to 1) -float64 charge_drawn # electric charge drawn from input (ampere-hour) -float64 charge_regen # electric charge regenerated to input (ampere-hour) +float64 charge_drawn # electric charge drawn from input (ampere-hours) +float64 charge_regen # electric charge regenerated to input (ampere-hour) amp_hours_charged float64 energy_drawn # energy drawn from input (watt-hour) -float64 energy_regen # energy regenerated to input (watt-hour) -float64 displacement # net tachometer (counts) -float64 distance_traveled # total tachnometer (counts) +float64 energy_regen # energy regenerated to input (watt_hours_charged) +int32 displacement # net tachometer (counts) tachometer +int32 distance_traveled # total tachnometer (counts) tachometer_abs int32 fault_code + +float64 pid_pos_now +int32 controller_id + + +float64 ntc_temp_mos1 +float64 ntc_temp_mos2 +float64 ntc_temp_mos3 +float64 avg_vd +float64 avg_vq + +#float64 temperature_pcb # temperature of printed circuit board (degrees Celsius) From 4126b55d5e128cbfac17b995f336669880e727af Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Mon, 24 May 2021 00:37:10 -0700 Subject: [PATCH 38/72] fix test results --- vesc_driver/src/vesc_driver.cpp | 8 +++++--- vesc_driver/src/vesc_interface.cpp | 4 ++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 1a237d4..c2135b5 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,9 +65,11 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - // default to /tmp/ttyV0 for debug purpose - // use socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' - // to sniff comunication data to and from vesc + /* default to /tmp/ttyV0 for debug purpose + * use : + * socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' + * to sniff comunication data to and from vesc + */ std::string port = declare_parameter("port", "/tmp/ttyV0"); // attempt to connect to the serial port diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 5e7a6f9..c73ce95 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -92,7 +92,7 @@ void VescInterface::Impl::packet_creation_thread() { buffer_mutex_.lock(); int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer_.empty()){ + if (!buffer_.empty()) { // search buffer for valid packet(s) auto iter = buffer_.begin(); auto iter_begin = buffer_.begin(); @@ -198,7 +198,7 @@ void VescInterface::connect(const std::string & port) impl_->connect(port); } catch (const std::exception & e) { std::stringstream ss; - ss << "Failed to open the serial port "<< port <<" to the VESC. " << e.what(); + ss << "Failed to open the serial port " << port << " to the VESC. " << e.what(); throw SerialException(ss.str().c_str()); } From 654ba11c65ec954c1cfabc771f7f0557b1641d1e Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Mon, 24 May 2021 02:33:45 -0700 Subject: [PATCH 39/72] fixed test results --- vesc_driver/include/vesc_driver/datatypes.hpp | 1700 +++++++++-------- .../include/vesc_driver/vesc_packet.hpp | 67 +- vesc_driver/src/vesc_driver.cpp | 54 +- vesc_driver/src/vesc_packet.cpp | 303 +-- 4 files changed, 1101 insertions(+), 1023 deletions(-) diff --git a/vesc_driver/include/vesc_driver/datatypes.hpp b/vesc_driver/include/vesc_driver/datatypes.hpp index c3043e4..8548f76 100644 --- a/vesc_driver/include/vesc_driver/datatypes.hpp +++ b/vesc_driver/include/vesc_driver/datatypes.hpp @@ -28,26 +28,26 @@ /* - Some parts of this code, Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se + Some parts of this code, Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se - This file is part of the VESC firmware. + This file is part of the VESC firmware. - The VESC firmware is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. + The VESC firmware is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. - The VESC firmware is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. + The VESC firmware is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ -#ifndef DATATYPES_H_ -#define DATATYPES_H_ +#ifndef VESC_DRIVER__DATATYPES_HPP_ +#define VESC_DRIVER__DATATYPES_HPP_ #include #include @@ -88,959 +88,1019 @@ typedef enum VESC_TX_DOUBLE32_AUTO } VESC_TX_T; -//#include "ch.h" -typedef uint32_t systime_t; // defined in ch.h +// #include "ch.h" +typedef uint32_t systime_t; // defined in ch.h // Data types -typedef enum { - MC_STATE_OFF = 0, - MC_STATE_DETECTING, - MC_STATE_RUNNING, - MC_STATE_FULL_BRAKE, +typedef enum +{ + MC_STATE_OFF = 0, + MC_STATE_DETECTING, + MC_STATE_RUNNING, + MC_STATE_FULL_BRAKE, } mc_state; -typedef enum { - PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended - PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode - PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs +typedef enum +{ + PWM_MODE_NONSYNCHRONOUS_HISW = 0, // This mode is not recommended + PWM_MODE_SYNCHRONOUS, // The recommended and most tested mode + PWM_MODE_BIPOLAR // Some glitches occasionally, can kill MOSFETs } mc_pwm_mode; -typedef enum { - COMM_MODE_INTEGRATE = 0, - COMM_MODE_DELAY +typedef enum +{ + COMM_MODE_INTEGRATE = 0, + COMM_MODE_DELAY } mc_comm_mode; -typedef enum { - SENSOR_MODE_SENSORLESS = 0, - SENSOR_MODE_SENSORED, - SENSOR_MODE_HYBRID +typedef enum +{ + SENSOR_MODE_SENSORLESS = 0, + SENSOR_MODE_SENSORED, + SENSOR_MODE_HYBRID } mc_sensor_mode; -typedef enum { - FOC_SENSOR_MODE_SENSORLESS = 0, - FOC_SENSOR_MODE_ENCODER, - FOC_SENSOR_MODE_HALL, - FOC_SENSOR_MODE_HFI +typedef enum +{ + FOC_SENSOR_MODE_SENSORLESS = 0, + FOC_SENSOR_MODE_ENCODER, + FOC_SENSOR_MODE_HALL, + FOC_SENSOR_MODE_HFI } mc_foc_sensor_mode; // Auxiliary output mode -typedef enum { - OUT_AUX_MODE_OFF = 0, - OUT_AUX_MODE_ON_AFTER_2S, - OUT_AUX_MODE_ON_AFTER_5S, - OUT_AUX_MODE_ON_AFTER_10S, - OUT_AUX_MODE_UNUSED +typedef enum +{ + OUT_AUX_MODE_OFF = 0, + OUT_AUX_MODE_ON_AFTER_2S, + OUT_AUX_MODE_ON_AFTER_5S, + OUT_AUX_MODE_ON_AFTER_10S, + OUT_AUX_MODE_UNUSED } out_aux_mode; // Temperature sensor type -typedef enum { - TEMP_SENSOR_NTC_10K_25C = 0, - TEMP_SENSOR_PTC_1K_100C, - TEMP_SENSOR_KTY83_122, +typedef enum +{ + TEMP_SENSOR_NTC_10K_25C = 0, + TEMP_SENSOR_PTC_1K_100C, + TEMP_SENSOR_KTY83_122, } temp_sensor_type; // General purpose drive output mode -typedef enum { - GPD_OUTPUT_MODE_NONE = 0, - GPD_OUTPUT_MODE_MODULATION, - GPD_OUTPUT_MODE_VOLTAGE, - GPD_OUTPUT_MODE_CURRENT +typedef enum +{ + GPD_OUTPUT_MODE_NONE = 0, + GPD_OUTPUT_MODE_MODULATION, + GPD_OUTPUT_MODE_VOLTAGE, + GPD_OUTPUT_MODE_CURRENT } gpd_output_mode; -typedef enum { - MOTOR_TYPE_BLDC = 0, - MOTOR_TYPE_DC, - MOTOR_TYPE_FOC, - MOTOR_TYPE_GPD +typedef enum +{ + MOTOR_TYPE_BLDC = 0, + MOTOR_TYPE_DC, + MOTOR_TYPE_FOC, + MOTOR_TYPE_GPD } mc_motor_type; // FOC current controller decoupling mode. -typedef enum { - FOC_CC_DECOUPLING_DISABLED = 0, - FOC_CC_DECOUPLING_CROSS, - FOC_CC_DECOUPLING_BEMF, - FOC_CC_DECOUPLING_CROSS_BEMF +typedef enum +{ + FOC_CC_DECOUPLING_DISABLED = 0, + FOC_CC_DECOUPLING_CROSS, + FOC_CC_DECOUPLING_BEMF, + FOC_CC_DECOUPLING_CROSS_BEMF } mc_foc_cc_decoupling_mode; -typedef enum { - FOC_OBSERVER_ORTEGA_ORIGINAL = 0, - FOC_OBSERVER_ORTEGA_ITERATIVE +typedef enum +{ + FOC_OBSERVER_ORTEGA_ORIGINAL = 0, + FOC_OBSERVER_ORTEGA_ITERATIVE } mc_foc_observer_type; -typedef enum { - FAULT_CODE_NONE = 0, - FAULT_CODE_OVER_VOLTAGE, - FAULT_CODE_UNDER_VOLTAGE, - FAULT_CODE_DRV, - FAULT_CODE_ABS_OVER_CURRENT, - FAULT_CODE_OVER_TEMP_FET, - FAULT_CODE_OVER_TEMP_MOTOR, - FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, - FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, - FAULT_CODE_MCU_UNDER_VOLTAGE, - FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, - FAULT_CODE_ENCODER_SPI, - FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, - FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, - FAULT_CODE_FLASH_CORRUPTION, - FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, - FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, - FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, - FAULT_CODE_UNBALANCED_CURRENTS, - FAULT_CODE_BRK, - FAULT_CODE_RESOLVER_LOT, - FAULT_CODE_RESOLVER_DOS, - FAULT_CODE_RESOLVER_LOS +typedef enum +{ + FAULT_CODE_NONE = 0, + FAULT_CODE_OVER_VOLTAGE, + FAULT_CODE_UNDER_VOLTAGE, + FAULT_CODE_DRV, + FAULT_CODE_ABS_OVER_CURRENT, + FAULT_CODE_OVER_TEMP_FET, + FAULT_CODE_OVER_TEMP_MOTOR, + FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, + FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, + FAULT_CODE_MCU_UNDER_VOLTAGE, + FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, + FAULT_CODE_ENCODER_SPI, + FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, + FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, + FAULT_CODE_FLASH_CORRUPTION, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, + FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, + FAULT_CODE_UNBALANCED_CURRENTS, + FAULT_CODE_BRK, + FAULT_CODE_RESOLVER_LOT, + FAULT_CODE_RESOLVER_DOS, + FAULT_CODE_RESOLVER_LOS } mc_fault_code; -typedef enum { - CONTROL_MODE_DUTY = 0, - CONTROL_MODE_SPEED, - CONTROL_MODE_CURRENT, - CONTROL_MODE_CURRENT_BRAKE, - CONTROL_MODE_POS, - CONTROL_MODE_HANDBRAKE, - CONTROL_MODE_OPENLOOP, - CONTROL_MODE_OPENLOOP_PHASE, - CONTROL_MODE_OPENLOOP_DUTY, - CONTROL_MODE_OPENLOOP_DUTY_PHASE, - CONTROL_MODE_NONE +typedef enum +{ + CONTROL_MODE_DUTY = 0, + CONTROL_MODE_SPEED, + CONTROL_MODE_CURRENT, + CONTROL_MODE_CURRENT_BRAKE, + CONTROL_MODE_POS, + CONTROL_MODE_HANDBRAKE, + CONTROL_MODE_OPENLOOP, + CONTROL_MODE_OPENLOOP_PHASE, + CONTROL_MODE_OPENLOOP_DUTY, + CONTROL_MODE_OPENLOOP_DUTY_PHASE, + CONTROL_MODE_NONE } mc_control_mode; -typedef enum { - DISP_POS_MODE_NONE = 0, - DISP_POS_MODE_INDUCTANCE, - DISP_POS_MODE_OBSERVER, - DISP_POS_MODE_ENCODER, - DISP_POS_MODE_PID_POS, - DISP_POS_MODE_PID_POS_ERROR, - DISP_POS_MODE_ENCODER_OBSERVER_ERROR +typedef enum +{ + DISP_POS_MODE_NONE = 0, + DISP_POS_MODE_INDUCTANCE, + DISP_POS_MODE_OBSERVER, + DISP_POS_MODE_ENCODER, + DISP_POS_MODE_PID_POS, + DISP_POS_MODE_PID_POS_ERROR, + DISP_POS_MODE_ENCODER_OBSERVER_ERROR } disp_pos_mode; -typedef enum { - SENSOR_PORT_MODE_HALL = 0, - SENSOR_PORT_MODE_ABI, - SENSOR_PORT_MODE_AS5047_SPI, - SENSOR_PORT_MODE_AD2S1205, - SENSOR_PORT_MODE_SINCOS, - SENSOR_PORT_MODE_TS5700N8501, - SENSOR_PORT_MODE_TS5700N8501_MULTITURN +typedef enum +{ + SENSOR_PORT_MODE_HALL = 0, + SENSOR_PORT_MODE_ABI, + SENSOR_PORT_MODE_AS5047_SPI, + SENSOR_PORT_MODE_AD2S1205, + SENSOR_PORT_MODE_SINCOS, + SENSOR_PORT_MODE_TS5700N8501, + SENSOR_PORT_MODE_TS5700N8501_MULTITURN } sensor_port_mode; -typedef struct { - float cycle_int_limit; - float cycle_int_limit_running; - float cycle_int_limit_max; - float comm_time_sum; - float comm_time_sum_min_rpm; - int32_t comms; - float time_at_comm; +typedef struct +{ + float cycle_int_limit; + float cycle_int_limit_running; + float cycle_int_limit_max; + float comm_time_sum; + float comm_time_sum_min_rpm; + int32_t comms; + float time_at_comm; } mc_rpm_dep_struct; -typedef enum { - DRV8301_OC_LIMIT = 0, - DRV8301_OC_LATCH_SHUTDOWN, - DRV8301_OC_REPORT_ONLY, - DRV8301_OC_DISABLED +typedef enum +{ + DRV8301_OC_LIMIT = 0, + DRV8301_OC_LATCH_SHUTDOWN, + DRV8301_OC_REPORT_ONLY, + DRV8301_OC_DISABLED } drv8301_oc_mode; -typedef enum { - DEBUG_SAMPLING_OFF = 0, - DEBUG_SAMPLING_NOW, - DEBUG_SAMPLING_START, - DEBUG_SAMPLING_TRIGGER_START, - DEBUG_SAMPLING_TRIGGER_FAULT, - DEBUG_SAMPLING_TRIGGER_START_NOSEND, - DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, - DEBUG_SAMPLING_SEND_LAST_SAMPLES +typedef enum +{ + DEBUG_SAMPLING_OFF = 0, + DEBUG_SAMPLING_NOW, + DEBUG_SAMPLING_START, + DEBUG_SAMPLING_TRIGGER_START, + DEBUG_SAMPLING_TRIGGER_FAULT, + DEBUG_SAMPLING_TRIGGER_START_NOSEND, + DEBUG_SAMPLING_TRIGGER_FAULT_NOSEND, + DEBUG_SAMPLING_SEND_LAST_SAMPLES } debug_sampling_mode; -typedef enum { - CAN_BAUD_125K = 0, - CAN_BAUD_250K, - CAN_BAUD_500K, - CAN_BAUD_1M, - CAN_BAUD_10K, - CAN_BAUD_20K, - CAN_BAUD_50K, - CAN_BAUD_75K +typedef enum +{ + CAN_BAUD_125K = 0, + CAN_BAUD_250K, + CAN_BAUD_500K, + CAN_BAUD_1M, + CAN_BAUD_10K, + CAN_BAUD_20K, + CAN_BAUD_50K, + CAN_BAUD_75K } CAN_BAUD; -typedef enum { - BATTERY_TYPE_LIION_3_0__4_2, - BATTERY_TYPE_LIIRON_2_6__3_6, - BATTERY_TYPE_LEAD_ACID +typedef enum +{ + BATTERY_TYPE_LIION_3_0__4_2, + BATTERY_TYPE_LIIRON_2_6__3_6, + BATTERY_TYPE_LEAD_ACID } BATTERY_TYPE; -typedef enum { - HFI_SAMPLES_8 = 0, - HFI_SAMPLES_16, - HFI_SAMPLES_32 +typedef enum +{ + HFI_SAMPLES_8 = 0, + HFI_SAMPLES_16, + HFI_SAMPLES_32 } FOC_HFI_SAMPLES; -typedef struct { - // Switching and drive - mc_pwm_mode pwm_mode; - mc_comm_mode comm_mode; - mc_motor_type motor_type; - mc_sensor_mode sensor_mode; - // Limits - float l_current_max; - float l_current_min; - float l_in_current_max; - float l_in_current_min; - float l_abs_current_max; - float l_min_erpm; - float l_max_erpm; - float l_erpm_start; - float l_max_erpm_fbrake; - float l_max_erpm_fbrake_cc; - float l_min_vin; - float l_max_vin; - float l_battery_cut_start; - float l_battery_cut_end; - bool l_slow_abs_current; - float l_temp_fet_start; - float l_temp_fet_end; - float l_temp_motor_start; - float l_temp_motor_end; - float l_temp_accel_dec; - float l_min_duty; - float l_max_duty; - float l_watt_max; - float l_watt_min; - float l_current_max_scale; - float l_current_min_scale; - float l_duty_start; - // Overridden limits (Computed during runtime) - float lo_current_max; - float lo_current_min; - float lo_in_current_max; - float lo_in_current_min; - float lo_current_motor_max_now; - float lo_current_motor_min_now; - // Sensorless (bldc) - float sl_min_erpm; - float sl_min_erpm_cycle_int_limit; - float sl_max_fullbreak_current_dir_change; - float sl_cycle_int_limit; - float sl_phase_advance_at_br; - float sl_cycle_int_rpm_br; - float sl_bemf_coupling_k; - // Hall sensor - int8_t hall_table[8]; - float hall_sl_erpm; - // FOC - float foc_current_kp; - float foc_current_ki; - float foc_f_sw; - float foc_dt_us; - float foc_encoder_offset; - bool foc_encoder_inverted; - float foc_encoder_ratio; - float foc_encoder_sin_offset; - float foc_encoder_sin_gain; - float foc_encoder_cos_offset; - float foc_encoder_cos_gain; - float foc_encoder_sincos_filter_constant; - float foc_motor_l; - float foc_motor_r; - float foc_motor_flux_linkage; - float foc_observer_gain; - float foc_observer_gain_slow; - float foc_pll_kp; - float foc_pll_ki; - float foc_duty_dowmramp_kp; - float foc_duty_dowmramp_ki; - float foc_openloop_rpm; - float foc_sl_openloop_hyst; - float foc_sl_openloop_time; - float foc_sl_d_current_duty; - float foc_sl_d_current_factor; - mc_foc_sensor_mode foc_sensor_mode; - uint8_t foc_hall_table[8]; - float foc_sl_erpm; - bool foc_sample_v0_v7; - bool foc_sample_high_current; - float foc_sat_comp; - bool foc_temp_comp; - float foc_temp_comp_base_temp; - float foc_current_filter_const; - mc_foc_cc_decoupling_mode foc_cc_decoupling; - mc_foc_observer_type foc_observer_type; - float foc_hfi_voltage_start; - float foc_hfi_voltage_run; - float foc_hfi_voltage_max; - float foc_sl_erpm_hfi; - uint16_t foc_hfi_start_samples; - float foc_hfi_obs_ovr_sec; - FOC_HFI_SAMPLES foc_hfi_samples; - // GPDrive - int gpd_buffer_notify_left; - int gpd_buffer_interpol; - float gpd_current_filter_const; - float gpd_current_kp; - float gpd_current_ki; - // Speed PID - float s_pid_kp; - float s_pid_ki; - float s_pid_kd; - float s_pid_kd_filter; - float s_pid_min_erpm; - bool s_pid_allow_braking; - // Pos PID - float p_pid_kp; - float p_pid_ki; - float p_pid_kd; - float p_pid_kd_filter; - float p_pid_ang_div; - // Current controller - float cc_startup_boost_duty; - float cc_min_current; - float cc_gain; - float cc_ramp_step_max; - // Misc - int32_t m_fault_stop_time_ms; - float m_duty_ramp_step; - float m_current_backoff_gain; - uint32_t m_encoder_counts; - sensor_port_mode m_sensor_port_mode; - bool m_invert_direction; - drv8301_oc_mode m_drv8301_oc_mode; - int m_drv8301_oc_adj; - float m_bldc_f_sw_min; - float m_bldc_f_sw_max; - float m_dc_f_sw; - float m_ntc_motor_beta; - out_aux_mode m_out_aux_mode; - temp_sensor_type m_motor_temp_sens_type; - float m_ptc_motor_coeff; - // Setup info - uint8_t si_motor_poles; - float si_gear_ratio; - float si_wheel_diameter; - BATTERY_TYPE si_battery_type; - int si_battery_cells; - float si_battery_ah; +typedef struct +{ + // Switching and drive + mc_pwm_mode pwm_mode; + mc_comm_mode comm_mode; + mc_motor_type motor_type; + mc_sensor_mode sensor_mode; + // Limits + float l_current_max; + float l_current_min; + float l_in_current_max; + float l_in_current_min; + float l_abs_current_max; + float l_min_erpm; + float l_max_erpm; + float l_erpm_start; + float l_max_erpm_fbrake; + float l_max_erpm_fbrake_cc; + float l_min_vin; + float l_max_vin; + float l_battery_cut_start; + float l_battery_cut_end; + bool l_slow_abs_current; + float l_temp_fet_start; + float l_temp_fet_end; + float l_temp_motor_start; + float l_temp_motor_end; + float l_temp_accel_dec; + float l_min_duty; + float l_max_duty; + float l_watt_max; + float l_watt_min; + float l_current_max_scale; + float l_current_min_scale; + float l_duty_start; + // Overridden limits (Computed during runtime) + float lo_current_max; + float lo_current_min; + float lo_in_current_max; + float lo_in_current_min; + float lo_current_motor_max_now; + float lo_current_motor_min_now; + // Sensorless (bldc) + float sl_min_erpm; + float sl_min_erpm_cycle_int_limit; + float sl_max_fullbreak_current_dir_change; + float sl_cycle_int_limit; + float sl_phase_advance_at_br; + float sl_cycle_int_rpm_br; + float sl_bemf_coupling_k; + // Hall sensor + int8_t hall_table[8]; + float hall_sl_erpm; + // FOC + float foc_current_kp; + float foc_current_ki; + float foc_f_sw; + float foc_dt_us; + float foc_encoder_offset; + bool foc_encoder_inverted; + float foc_encoder_ratio; + float foc_encoder_sin_offset; + float foc_encoder_sin_gain; + float foc_encoder_cos_offset; + float foc_encoder_cos_gain; + float foc_encoder_sincos_filter_constant; + float foc_motor_l; + float foc_motor_r; + float foc_motor_flux_linkage; + float foc_observer_gain; + float foc_observer_gain_slow; + float foc_pll_kp; + float foc_pll_ki; + float foc_duty_dowmramp_kp; + float foc_duty_dowmramp_ki; + float foc_openloop_rpm; + float foc_sl_openloop_hyst; + float foc_sl_openloop_time; + float foc_sl_d_current_duty; + float foc_sl_d_current_factor; + mc_foc_sensor_mode foc_sensor_mode; + uint8_t foc_hall_table[8]; + float foc_sl_erpm; + bool foc_sample_v0_v7; + bool foc_sample_high_current; + float foc_sat_comp; + bool foc_temp_comp; + float foc_temp_comp_base_temp; + float foc_current_filter_const; + mc_foc_cc_decoupling_mode foc_cc_decoupling; + mc_foc_observer_type foc_observer_type; + float foc_hfi_voltage_start; + float foc_hfi_voltage_run; + float foc_hfi_voltage_max; + float foc_sl_erpm_hfi; + uint16_t foc_hfi_start_samples; + float foc_hfi_obs_ovr_sec; + FOC_HFI_SAMPLES foc_hfi_samples; + // GPDrive + int gpd_buffer_notify_left; + int gpd_buffer_interpol; + float gpd_current_filter_const; + float gpd_current_kp; + float gpd_current_ki; + // Speed PID + float s_pid_kp; + float s_pid_ki; + float s_pid_kd; + float s_pid_kd_filter; + float s_pid_min_erpm; + bool s_pid_allow_braking; + // Pos PID + float p_pid_kp; + float p_pid_ki; + float p_pid_kd; + float p_pid_kd_filter; + float p_pid_ang_div; + // Current controller + float cc_startup_boost_duty; + float cc_min_current; + float cc_gain; + float cc_ramp_step_max; + // Misc + int32_t m_fault_stop_time_ms; + float m_duty_ramp_step; + float m_current_backoff_gain; + uint32_t m_encoder_counts; + sensor_port_mode m_sensor_port_mode; + bool m_invert_direction; + drv8301_oc_mode m_drv8301_oc_mode; + int m_drv8301_oc_adj; + float m_bldc_f_sw_min; + float m_bldc_f_sw_max; + float m_dc_f_sw; + float m_ntc_motor_beta; + out_aux_mode m_out_aux_mode; + temp_sensor_type m_motor_temp_sens_type; + float m_ptc_motor_coeff; + // Setup info + uint8_t si_motor_poles; + float si_gear_ratio; + float si_wheel_diameter; + BATTERY_TYPE si_battery_type; + int si_battery_cells; + float si_battery_ah; } mc_configuration; // Applications to use -typedef enum { - APP_NONE = 0, - APP_PPM, - APP_ADC, - APP_UART, - APP_PPM_UART, - APP_ADC_UART, - APP_NUNCHUK, - APP_NRF, - APP_CUSTOM, - APP_BALANCE +typedef enum +{ + APP_NONE = 0, + APP_PPM, + APP_ADC, + APP_UART, + APP_PPM_UART, + APP_ADC_UART, + APP_NUNCHUK, + APP_NRF, + APP_CUSTOM, + APP_BALANCE } app_use; // Throttle curve mode -typedef enum { - THR_EXP_EXPO = 0, - THR_EXP_NATURAL, - THR_EXP_POLY +typedef enum +{ + THR_EXP_EXPO = 0, + THR_EXP_NATURAL, + THR_EXP_POLY } thr_exp_mode; // PPM control types -typedef enum { - PPM_CTRL_TYPE_NONE = 0, - PPM_CTRL_TYPE_CURRENT, - PPM_CTRL_TYPE_CURRENT_NOREV, - PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, - PPM_CTRL_TYPE_DUTY, - PPM_CTRL_TYPE_DUTY_NOREV, - PPM_CTRL_TYPE_PID, - PPM_CTRL_TYPE_PID_NOREV, - PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST, - PPM_CTRL_TYPE_CURRENT_SMART_REV +typedef enum +{ + PPM_CTRL_TYPE_NONE = 0, + PPM_CTRL_TYPE_CURRENT, + PPM_CTRL_TYPE_CURRENT_NOREV, + PPM_CTRL_TYPE_CURRENT_NOREV_BRAKE, + PPM_CTRL_TYPE_DUTY, + PPM_CTRL_TYPE_DUTY_NOREV, + PPM_CTRL_TYPE_PID, + PPM_CTRL_TYPE_PID_NOREV, + PPM_CTRL_TYPE_CURRENT_BRAKE_REV_HYST, + PPM_CTRL_TYPE_CURRENT_SMART_REV } ppm_control_type; -typedef struct { - ppm_control_type ctrl_type; - float pid_max_erpm; - float hyst; - float pulse_start; - float pulse_end; - float pulse_center; - bool median_filter; - bool safe_start; - float throttle_exp; - float throttle_exp_brake; - thr_exp_mode throttle_exp_mode; - float ramp_time_pos; - float ramp_time_neg; - bool multi_esc; - bool tc; - float tc_max_diff; - float max_erpm_for_dir; - float smart_rev_max_duty; - float smart_rev_ramp_time; +typedef struct +{ + ppm_control_type ctrl_type; + float pid_max_erpm; + float hyst; + float pulse_start; + float pulse_end; + float pulse_center; + bool median_filter; + bool safe_start; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + float max_erpm_for_dir; + float smart_rev_max_duty; + float smart_rev_ramp_time; } ppm_config; // ADC control types -typedef enum { - ADC_CTRL_TYPE_NONE = 0, - ADC_CTRL_TYPE_CURRENT, - ADC_CTRL_TYPE_CURRENT_REV_CENTER, - ADC_CTRL_TYPE_CURRENT_REV_BUTTON, - ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, - ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, - ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, - ADC_CTRL_TYPE_DUTY, - ADC_CTRL_TYPE_DUTY_REV_CENTER, - ADC_CTRL_TYPE_DUTY_REV_BUTTON, - ADC_CTRL_TYPE_PID, - ADC_CTRL_TYPE_PID_REV_CENTER, - ADC_CTRL_TYPE_PID_REV_BUTTON +typedef enum +{ + ADC_CTRL_TYPE_NONE = 0, + ADC_CTRL_TYPE_CURRENT, + ADC_CTRL_TYPE_CURRENT_REV_CENTER, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_ADC, + ADC_CTRL_TYPE_CURRENT_REV_BUTTON_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON, + ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_ADC, + ADC_CTRL_TYPE_DUTY, + ADC_CTRL_TYPE_DUTY_REV_CENTER, + ADC_CTRL_TYPE_DUTY_REV_BUTTON, + ADC_CTRL_TYPE_PID, + ADC_CTRL_TYPE_PID_REV_CENTER, + ADC_CTRL_TYPE_PID_REV_BUTTON } adc_control_type; -typedef struct { - adc_control_type ctrl_type; - float hyst; - float voltage_start; - float voltage_end; - float voltage_center; - float voltage2_start; - float voltage2_end; - bool use_filter; - bool safe_start; - bool cc_button_inverted; - bool rev_button_inverted; - bool voltage_inverted; - bool voltage2_inverted; - float throttle_exp; - float throttle_exp_brake; - thr_exp_mode throttle_exp_mode; - float ramp_time_pos; - float ramp_time_neg; - bool multi_esc; - bool tc; - float tc_max_diff; - uint32_t update_rate_hz; +typedef struct +{ + adc_control_type ctrl_type; + float hyst; + float voltage_start; + float voltage_end; + float voltage_center; + float voltage2_start; + float voltage2_end; + bool use_filter; + bool safe_start; + bool cc_button_inverted; + bool rev_button_inverted; + bool voltage_inverted; + bool voltage2_inverted; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + float ramp_time_pos; + float ramp_time_neg; + bool multi_esc; + bool tc; + float tc_max_diff; + uint32_t update_rate_hz; } adc_config; // Nunchuk control types -typedef enum { - CHUK_CTRL_TYPE_NONE = 0, - CHUK_CTRL_TYPE_CURRENT, - CHUK_CTRL_TYPE_CURRENT_NOREV +typedef enum +{ + CHUK_CTRL_TYPE_NONE = 0, + CHUK_CTRL_TYPE_CURRENT, + CHUK_CTRL_TYPE_CURRENT_NOREV } chuk_control_type; -typedef struct { - chuk_control_type ctrl_type; - float hyst; - float ramp_time_pos; - float ramp_time_neg; - float stick_erpm_per_s_in_cc; - float throttle_exp; - float throttle_exp_brake; - thr_exp_mode throttle_exp_mode; - bool multi_esc; - bool tc; - float tc_max_diff; - bool use_smart_rev; - float smart_rev_max_duty; - float smart_rev_ramp_time; +typedef struct +{ + chuk_control_type ctrl_type; + float hyst; + float ramp_time_pos; + float ramp_time_neg; + float stick_erpm_per_s_in_cc; + float throttle_exp; + float throttle_exp_brake; + thr_exp_mode throttle_exp_mode; + bool multi_esc; + bool tc; + float tc_max_diff; + bool use_smart_rev; + float smart_rev_max_duty; + float smart_rev_ramp_time; } chuk_config; // NRF Datatypes -typedef enum { - NRF_SPEED_250K = 0, - NRF_SPEED_1M, - NRF_SPEED_2M +typedef enum +{ + NRF_SPEED_250K = 0, + NRF_SPEED_1M, + NRF_SPEED_2M } NRF_SPEED; -typedef enum { - NRF_POWER_M18DBM = 0, - NRF_POWER_M12DBM, - NRF_POWER_M6DBM, - NRF_POWER_0DBM, +typedef enum +{ + NRF_POWER_M18DBM = 0, + NRF_POWER_M12DBM, + NRF_POWER_M6DBM, + NRF_POWER_0DBM, NRF_POWER_OFF } NRF_POWER; -typedef enum { - NRF_AW_3 = 0, - NRF_AW_4, - NRF_AW_5 +typedef enum +{ + NRF_AW_3 = 0, + NRF_AW_4, + NRF_AW_5 } NRF_AW; -typedef enum { - NRF_CRC_DISABLED = 0, - NRF_CRC_1B, - NRF_CRC_2B +typedef enum +{ + NRF_CRC_DISABLED = 0, + NRF_CRC_1B, + NRF_CRC_2B } NRF_CRC; -typedef enum { - NRF_RETR_DELAY_250US = 0, - NRF_RETR_DELAY_500US, - NRF_RETR_DELAY_750US, - NRF_RETR_DELAY_1000US, - NRF_RETR_DELAY_1250US, - NRF_RETR_DELAY_1500US, - NRF_RETR_DELAY_1750US, - NRF_RETR_DELAY_2000US, - NRF_RETR_DELAY_2250US, - NRF_RETR_DELAY_2500US, - NRF_RETR_DELAY_2750US, - NRF_RETR_DELAY_3000US, - NRF_RETR_DELAY_3250US, - NRF_RETR_DELAY_3500US, - NRF_RETR_DELAY_3750US, - NRF_RETR_DELAY_4000US +typedef enum +{ + NRF_RETR_DELAY_250US = 0, + NRF_RETR_DELAY_500US, + NRF_RETR_DELAY_750US, + NRF_RETR_DELAY_1000US, + NRF_RETR_DELAY_1250US, + NRF_RETR_DELAY_1500US, + NRF_RETR_DELAY_1750US, + NRF_RETR_DELAY_2000US, + NRF_RETR_DELAY_2250US, + NRF_RETR_DELAY_2500US, + NRF_RETR_DELAY_2750US, + NRF_RETR_DELAY_3000US, + NRF_RETR_DELAY_3250US, + NRF_RETR_DELAY_3500US, + NRF_RETR_DELAY_3750US, + NRF_RETR_DELAY_4000US } NRF_RETR_DELAY; -typedef struct { - NRF_SPEED speed; - NRF_POWER power; - NRF_CRC crc_type; - NRF_RETR_DELAY retry_delay; - unsigned char retries; - unsigned char channel; - unsigned char address[3]; - bool send_crc_ack; +typedef struct +{ + NRF_SPEED speed; + NRF_POWER power; + NRF_CRC crc_type; + NRF_RETR_DELAY retry_delay; + unsigned char retries; + unsigned char channel; + unsigned char address[3]; + bool send_crc_ack; } nrf_config; -typedef struct { - float kp; - float ki; - float kd; - uint16_t hertz; - float pitch_fault; - float roll_fault; - float adc1; - float adc2; - float overspeed_duty; - float tiltback_duty; - float tiltback_angle; - float tiltback_speed; - float tiltback_high_voltage; - float tiltback_low_voltage; - float startup_pitch_tolerance; - float startup_roll_tolerance; - float startup_speed; - float deadzone; - float current_boost; - bool multi_esc; - float yaw_kp; - float yaw_ki; - float yaw_kd; - float roll_steer_kp; - float brake_current; - uint16_t overspeed_delay; - uint16_t fault_delay; - float tiltback_constant; - float roll_steer_erpm_kp; - float yaw_current_clamp; - uint16_t adc_half_fault_erpm; - float setpoint_pitch_filter; - float setpoint_target_filter; - float setpoint_clamp; +typedef struct +{ + float kp; + float ki; + float kd; + uint16_t hertz; + float pitch_fault; + float roll_fault; + float adc1; + float adc2; + float overspeed_duty; + float tiltback_duty; + float tiltback_angle; + float tiltback_speed; + float tiltback_high_voltage; + float tiltback_low_voltage; + float startup_pitch_tolerance; + float startup_roll_tolerance; + float startup_speed; + float deadzone; + float current_boost; + bool multi_esc; + float yaw_kp; + float yaw_ki; + float yaw_kd; + float roll_steer_kp; + float brake_current; + uint16_t overspeed_delay; + uint16_t fault_delay; + float tiltback_constant; + float roll_steer_erpm_kp; + float yaw_current_clamp; + uint16_t adc_half_fault_erpm; + float setpoint_pitch_filter; + float setpoint_target_filter; + float setpoint_clamp; } balance_config; // CAN status modes -typedef enum { - CAN_STATUS_DISABLED = 0, - CAN_STATUS_1, - CAN_STATUS_1_2, - CAN_STATUS_1_2_3, - CAN_STATUS_1_2_3_4, - CAN_STATUS_1_2_3_4_5 +typedef enum +{ + CAN_STATUS_DISABLED = 0, + CAN_STATUS_1, + CAN_STATUS_1_2, + CAN_STATUS_1_2_3, + CAN_STATUS_1_2_3_4, + CAN_STATUS_1_2_3_4_5 } CAN_STATUS_MODE; -typedef enum { - SHUTDOWN_MODE_ALWAYS_OFF = 0, - SHUTDOWN_MODE_ALWAYS_ON, - SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY, - SHUTDOWN_MODE_OFF_AFTER_10S, - SHUTDOWN_MODE_OFF_AFTER_1M, - SHUTDOWN_MODE_OFF_AFTER_5M, - SHUTDOWN_MODE_OFF_AFTER_10M, - SHUTDOWN_MODE_OFF_AFTER_30M, - SHUTDOWN_MODE_OFF_AFTER_1H, - SHUTDOWN_MODE_OFF_AFTER_5H, +typedef enum +{ + SHUTDOWN_MODE_ALWAYS_OFF = 0, + SHUTDOWN_MODE_ALWAYS_ON, + SHUTDOWN_MODE_TOGGLE_BUTTON_ONLY, + SHUTDOWN_MODE_OFF_AFTER_10S, + SHUTDOWN_MODE_OFF_AFTER_1M, + SHUTDOWN_MODE_OFF_AFTER_5M, + SHUTDOWN_MODE_OFF_AFTER_10M, + SHUTDOWN_MODE_OFF_AFTER_30M, + SHUTDOWN_MODE_OFF_AFTER_1H, + SHUTDOWN_MODE_OFF_AFTER_5H, } SHUTDOWN_MODE; -typedef enum { - IMU_TYPE_OFF = 0, - IMU_TYPE_INTERNAL, - IMU_TYPE_EXTERNAL_MPU9X50, - IMU_TYPE_EXTERNAL_ICM20948, - IMU_TYPE_EXTERNAL_BMI160 +typedef enum +{ + IMU_TYPE_OFF = 0, + IMU_TYPE_INTERNAL, + IMU_TYPE_EXTERNAL_MPU9X50, + IMU_TYPE_EXTERNAL_ICM20948, + IMU_TYPE_EXTERNAL_BMI160 } IMU_TYPE; -typedef enum { - AHRS_MODE_MADGWICK = 0, - AHRS_MODE_MAHONY +typedef enum +{ + AHRS_MODE_MADGWICK = 0, + AHRS_MODE_MAHONY } AHRS_MODE; -typedef struct { - IMU_TYPE type; - AHRS_MODE mode; - int sample_rate_hz; - float accel_confidence_decay; - float mahony_kp; - float mahony_ki; - float madgwick_beta; - float rot_roll; - float rot_pitch; - float rot_yaw; - float accel_offsets[3]; - float gyro_offsets[3]; - float gyro_offset_comp_fact[3]; - float gyro_offset_comp_clamp; +typedef struct +{ + IMU_TYPE type; + AHRS_MODE mode; + int sample_rate_hz; + float accel_confidence_decay; + float mahony_kp; + float mahony_ki; + float madgwick_beta; + float rot_roll; + float rot_pitch; + float rot_yaw; + float accel_offsets[3]; + float gyro_offsets[3]; + float gyro_offset_comp_fact[3]; + float gyro_offset_comp_clamp; } imu_config; -typedef enum { - CAN_MODE_VESC = 0, - CAN_MODE_UAVCAN, - CAN_MODE_COMM_BRIDGE +typedef enum +{ + CAN_MODE_VESC = 0, + CAN_MODE_UAVCAN, + CAN_MODE_COMM_BRIDGE } CAN_MODE; -typedef struct { - // Settings - uint8_t controller_id; - uint32_t timeout_msec; - float timeout_brake_current; - CAN_STATUS_MODE send_can_status; - uint32_t send_can_status_rate_hz; - CAN_BAUD can_baud_rate; - bool pairing_done; - bool permanent_uart_enabled; - SHUTDOWN_MODE shutdown_mode; +typedef struct +{ + // Settings + uint8_t controller_id; + uint32_t timeout_msec; + float timeout_brake_current; + CAN_STATUS_MODE send_can_status; + uint32_t send_can_status_rate_hz; + CAN_BAUD can_baud_rate; + bool pairing_done; + bool permanent_uart_enabled; + SHUTDOWN_MODE shutdown_mode; - // CAN modes - CAN_MODE can_mode; - uint8_t uavcan_esc_index; + // CAN modes + CAN_MODE can_mode; + uint8_t uavcan_esc_index; - // Application to use - app_use app_to_use; + // Application to use + app_use app_to_use; - // PPM application settings - ppm_config app_ppm_conf; + // PPM application settings + ppm_config app_ppm_conf; - // ADC application settings - adc_config app_adc_conf; + // ADC application settings + adc_config app_adc_conf; - // UART application settings - uint32_t app_uart_baudrate; + // UART application settings + uint32_t app_uart_baudrate; - // Nunchuk application settings - chuk_config app_chuk_conf; + // Nunchuk application settings + chuk_config app_chuk_conf; - // NRF application settings - nrf_config app_nrf_conf; + // NRF application settings + nrf_config app_nrf_conf; - // Balance application settings - balance_config app_balance_conf; + // Balance application settings + balance_config app_balance_conf; - // IMU Settings - imu_config imu_conf; + // IMU Settings + imu_config imu_conf; } app_configuration; // Communication commands -typedef enum { - COMM_FW_VERSION = 0, - COMM_JUMP_TO_BOOTLOADER, - COMM_ERASE_NEW_APP, - COMM_WRITE_NEW_APP_DATA, - COMM_GET_VALUES, - COMM_SET_DUTY, - COMM_SET_CURRENT, - COMM_SET_CURRENT_BRAKE, - COMM_SET_RPM, - COMM_SET_POS, - COMM_SET_HANDBRAKE, - COMM_SET_DETECT, - COMM_SET_SERVO_POS, - COMM_SET_MCCONF, - COMM_GET_MCCONF, - COMM_GET_MCCONF_DEFAULT, - COMM_SET_APPCONF, - COMM_GET_APPCONF, - COMM_GET_APPCONF_DEFAULT, - COMM_SAMPLE_PRINT, - COMM_TERMINAL_CMD, - COMM_PRINT, - COMM_ROTOR_POSITION, - COMM_EXPERIMENT_SAMPLE, - COMM_DETECT_MOTOR_PARAM, - COMM_DETECT_MOTOR_R_L, - COMM_DETECT_MOTOR_FLUX_LINKAGE, - COMM_DETECT_ENCODER, - COMM_DETECT_HALL_FOC, - COMM_REBOOT, - COMM_ALIVE, - COMM_GET_DECODED_PPM, - COMM_GET_DECODED_ADC, - COMM_GET_DECODED_CHUK, - COMM_FORWARD_CAN, - COMM_SET_CHUCK_DATA, - COMM_CUSTOM_APP_DATA, - COMM_NRF_START_PAIRING, - COMM_GPD_SET_FSW, - COMM_GPD_BUFFER_NOTIFY, - COMM_GPD_BUFFER_SIZE_LEFT, - COMM_GPD_FILL_BUFFER, - COMM_GPD_OUTPUT_SAMPLE, - COMM_GPD_SET_MODE, - COMM_GPD_FILL_BUFFER_INT8, - COMM_GPD_FILL_BUFFER_INT16, - COMM_GPD_SET_BUFFER_INT_SCALE, - COMM_GET_VALUES_SETUP, - COMM_SET_MCCONF_TEMP, - COMM_SET_MCCONF_TEMP_SETUP, - COMM_GET_VALUES_SELECTIVE, - COMM_GET_VALUES_SETUP_SELECTIVE, - COMM_EXT_NRF_PRESENT, - COMM_EXT_NRF_ESB_SET_CH_ADDR, - COMM_EXT_NRF_ESB_SEND_DATA, - COMM_EXT_NRF_ESB_RX_DATA, - COMM_EXT_NRF_SET_ENABLED, - COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, - COMM_DETECT_APPLY_ALL_FOC, - COMM_JUMP_TO_BOOTLOADER_ALL_CAN, - COMM_ERASE_NEW_APP_ALL_CAN, - COMM_WRITE_NEW_APP_DATA_ALL_CAN, - COMM_PING_CAN, - COMM_APP_DISABLE_OUTPUT, - COMM_TERMINAL_CMD_SYNC, - COMM_GET_IMU_DATA, - COMM_BM_CONNECT, - COMM_BM_ERASE_FLASH_ALL, - COMM_BM_WRITE_FLASH, - COMM_BM_REBOOT, - COMM_BM_DISCONNECT, - COMM_BM_MAP_PINS_DEFAULT, - COMM_BM_MAP_PINS_NRF5X, - COMM_ERASE_BOOTLOADER, - COMM_ERASE_BOOTLOADER_ALL_CAN, - COMM_PLOT_INIT, - COMM_PLOT_DATA, - COMM_PLOT_ADD_GRAPH, - COMM_PLOT_SET_GRAPH, - COMM_GET_DECODED_BALANCE, - COMM_BM_MEM_READ, - COMM_WRITE_NEW_APP_DATA_LZO, - COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, - COMM_BM_WRITE_FLASH_LZO, - COMM_SET_CURRENT_REL, - COMM_CAN_FWD_FRAME, - COMM_SET_BATTERY_CUT, - COMM_SET_BLE_NAME, - COMM_SET_BLE_PIN, - COMM_SET_CAN_MODE, - COMM_GET_IMU_CALIBRATION +typedef enum +{ + COMM_FW_VERSION = 0, + COMM_JUMP_TO_BOOTLOADER, + COMM_ERASE_NEW_APP, + COMM_WRITE_NEW_APP_DATA, + COMM_GET_VALUES, + COMM_SET_DUTY, + COMM_SET_CURRENT, + COMM_SET_CURRENT_BRAKE, + COMM_SET_RPM, + COMM_SET_POS, + COMM_SET_HANDBRAKE, + COMM_SET_DETECT, + COMM_SET_SERVO_POS, + COMM_SET_MCCONF, + COMM_GET_MCCONF, + COMM_GET_MCCONF_DEFAULT, + COMM_SET_APPCONF, + COMM_GET_APPCONF, + COMM_GET_APPCONF_DEFAULT, + COMM_SAMPLE_PRINT, + COMM_TERMINAL_CMD, + COMM_PRINT, + COMM_ROTOR_POSITION, + COMM_EXPERIMENT_SAMPLE, + COMM_DETECT_MOTOR_PARAM, + COMM_DETECT_MOTOR_R_L, + COMM_DETECT_MOTOR_FLUX_LINKAGE, + COMM_DETECT_ENCODER, + COMM_DETECT_HALL_FOC, + COMM_REBOOT, + COMM_ALIVE, + COMM_GET_DECODED_PPM, + COMM_GET_DECODED_ADC, + COMM_GET_DECODED_CHUK, + COMM_FORWARD_CAN, + COMM_SET_CHUCK_DATA, + COMM_CUSTOM_APP_DATA, + COMM_NRF_START_PAIRING, + COMM_GPD_SET_FSW, + COMM_GPD_BUFFER_NOTIFY, + COMM_GPD_BUFFER_SIZE_LEFT, + COMM_GPD_FILL_BUFFER, + COMM_GPD_OUTPUT_SAMPLE, + COMM_GPD_SET_MODE, + COMM_GPD_FILL_BUFFER_INT8, + COMM_GPD_FILL_BUFFER_INT16, + COMM_GPD_SET_BUFFER_INT_SCALE, + COMM_GET_VALUES_SETUP, + COMM_SET_MCCONF_TEMP, + COMM_SET_MCCONF_TEMP_SETUP, + COMM_GET_VALUES_SELECTIVE, + COMM_GET_VALUES_SETUP_SELECTIVE, + COMM_EXT_NRF_PRESENT, + COMM_EXT_NRF_ESB_SET_CH_ADDR, + COMM_EXT_NRF_ESB_SEND_DATA, + COMM_EXT_NRF_ESB_RX_DATA, + COMM_EXT_NRF_SET_ENABLED, + COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + COMM_DETECT_APPLY_ALL_FOC, + COMM_JUMP_TO_BOOTLOADER_ALL_CAN, + COMM_ERASE_NEW_APP_ALL_CAN, + COMM_WRITE_NEW_APP_DATA_ALL_CAN, + COMM_PING_CAN, + COMM_APP_DISABLE_OUTPUT, + COMM_TERMINAL_CMD_SYNC, + COMM_GET_IMU_DATA, + COMM_BM_CONNECT, + COMM_BM_ERASE_FLASH_ALL, + COMM_BM_WRITE_FLASH, + COMM_BM_REBOOT, + COMM_BM_DISCONNECT, + COMM_BM_MAP_PINS_DEFAULT, + COMM_BM_MAP_PINS_NRF5X, + COMM_ERASE_BOOTLOADER, + COMM_ERASE_BOOTLOADER_ALL_CAN, + COMM_PLOT_INIT, + COMM_PLOT_DATA, + COMM_PLOT_ADD_GRAPH, + COMM_PLOT_SET_GRAPH, + COMM_GET_DECODED_BALANCE, + COMM_BM_MEM_READ, + COMM_WRITE_NEW_APP_DATA_LZO, + COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO, + COMM_BM_WRITE_FLASH_LZO, + COMM_SET_CURRENT_REL, + COMM_CAN_FWD_FRAME, + COMM_SET_BATTERY_CUT, + COMM_SET_BLE_NAME, + COMM_SET_BLE_PIN, + COMM_SET_CAN_MODE, + COMM_GET_IMU_CALIBRATION } COMM_PACKET_ID; // CAN commands -typedef enum { - CAN_PACKET_SET_DUTY = 0, - CAN_PACKET_SET_CURRENT, - CAN_PACKET_SET_CURRENT_BRAKE, - CAN_PACKET_SET_RPM, - CAN_PACKET_SET_POS, - CAN_PACKET_FILL_RX_BUFFER, - CAN_PACKET_FILL_RX_BUFFER_LONG, - CAN_PACKET_PROCESS_RX_BUFFER, - CAN_PACKET_PROCESS_SHORT_BUFFER, - CAN_PACKET_STATUS, - CAN_PACKET_SET_CURRENT_REL, - CAN_PACKET_SET_CURRENT_BRAKE_REL, - CAN_PACKET_SET_CURRENT_HANDBRAKE, - CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, - CAN_PACKET_STATUS_2, - CAN_PACKET_STATUS_3, - CAN_PACKET_STATUS_4, - CAN_PACKET_PING, - CAN_PACKET_PONG, - CAN_PACKET_DETECT_APPLY_ALL_FOC, - CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, - CAN_PACKET_CONF_CURRENT_LIMITS, - CAN_PACKET_CONF_STORE_CURRENT_LIMITS, - CAN_PACKET_CONF_CURRENT_LIMITS_IN, - CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, - CAN_PACKET_CONF_FOC_ERPMS, - CAN_PACKET_CONF_STORE_FOC_ERPMS, - CAN_PACKET_STATUS_5, - CAN_PACKET_POLL_TS5700N8501_STATUS, - CAN_PACKET_CONF_BATTERY_CUT, - CAN_PACKET_CONF_STORE_BATTERY_CUT, - CAN_PACKET_SHUTDOWN +typedef enum +{ + CAN_PACKET_SET_DUTY = 0, + CAN_PACKET_SET_CURRENT, + CAN_PACKET_SET_CURRENT_BRAKE, + CAN_PACKET_SET_RPM, + CAN_PACKET_SET_POS, + CAN_PACKET_FILL_RX_BUFFER, + CAN_PACKET_FILL_RX_BUFFER_LONG, + CAN_PACKET_PROCESS_RX_BUFFER, + CAN_PACKET_PROCESS_SHORT_BUFFER, + CAN_PACKET_STATUS, + CAN_PACKET_SET_CURRENT_REL, + CAN_PACKET_SET_CURRENT_BRAKE_REL, + CAN_PACKET_SET_CURRENT_HANDBRAKE, + CAN_PACKET_SET_CURRENT_HANDBRAKE_REL, + CAN_PACKET_STATUS_2, + CAN_PACKET_STATUS_3, + CAN_PACKET_STATUS_4, + CAN_PACKET_PING, + CAN_PACKET_PONG, + CAN_PACKET_DETECT_APPLY_ALL_FOC, + CAN_PACKET_DETECT_APPLY_ALL_FOC_RES, + CAN_PACKET_CONF_CURRENT_LIMITS, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS, + CAN_PACKET_CONF_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN, + CAN_PACKET_CONF_FOC_ERPMS, + CAN_PACKET_CONF_STORE_FOC_ERPMS, + CAN_PACKET_STATUS_5, + CAN_PACKET_POLL_TS5700N8501_STATUS, + CAN_PACKET_CONF_BATTERY_CUT, + CAN_PACKET_CONF_STORE_BATTERY_CUT, + CAN_PACKET_SHUTDOWN } CAN_PACKET_ID; // Logged fault data -typedef struct { - uint8_t motor; - mc_fault_code fault; - float current; - float current_filtered; - float voltage; - float gate_driver_voltage; - float duty; - float rpm; - int tacho; - int cycles_running; - int tim_val_samp; - int tim_current_samp; - int tim_top; - int comm_step; - float temperature; - int drv8301_faults; +typedef struct +{ + uint8_t motor; + mc_fault_code fault; + float current; + float current_filtered; + float voltage; + float gate_driver_voltage; + float duty; + float rpm; + int tacho; + int cycles_running; + int tim_val_samp; + int tim_current_samp; + int tim_top; + int comm_step; + float temperature; + int drv8301_faults; } fault_data; // External LED state -typedef enum { - LED_EXT_OFF = 0, - LED_EXT_NORMAL, - LED_EXT_BRAKE, - LED_EXT_TURN_LEFT, - LED_EXT_TURN_RIGHT, - LED_EXT_BRAKE_TURN_LEFT, - LED_EXT_BRAKE_TURN_RIGHT, - LED_EXT_BATT +typedef enum +{ + LED_EXT_OFF = 0, + LED_EXT_NORMAL, + LED_EXT_BRAKE, + LED_EXT_TURN_LEFT, + LED_EXT_TURN_RIGHT, + LED_EXT_BRAKE_TURN_LEFT, + LED_EXT_BRAKE_TURN_RIGHT, + LED_EXT_BATT } LED_EXT_STATE; -typedef struct { - int js_x; - int js_y; - int acc_x; - int acc_y; - int acc_z; - bool bt_c; - bool bt_z; - bool rev_has_state; - bool is_rev; +typedef struct +{ + int js_x; + int js_y; + int acc_x; + int acc_y; + int acc_z; + bool bt_c; + bool bt_z; + bool rev_has_state; + bool is_rev; } chuck_data; -typedef struct { - int id; - systime_t rx_time; - float rpm; - float current; - float duty; +typedef struct +{ + int id; + systime_t rx_time; + float rpm; + float current; + float duty; } can_status_msg; -typedef struct { - int id; - systime_t rx_time; - float amp_hours; - float amp_hours_charged; +typedef struct +{ + int id; + systime_t rx_time; + float amp_hours; + float amp_hours_charged; } can_status_msg_2; -typedef struct { - int id; - systime_t rx_time; - float watt_hours; - float watt_hours_charged; +typedef struct +{ + int id; + systime_t rx_time; + float watt_hours; + float watt_hours_charged; } can_status_msg_3; -typedef struct { - int id; - systime_t rx_time; - float temp_fet; - float temp_motor; - float current_in; - float pid_pos_now; +typedef struct +{ + int id; + systime_t rx_time; + float temp_fet; + float temp_motor; + float current_in; + float pid_pos_now; } can_status_msg_4; -typedef struct { - int id; - systime_t rx_time; - float v_in; - int32_t tacho_value; +typedef struct +{ + int id; + systime_t rx_time; + float v_in; + int32_t tacho_value; } can_status_msg_5; -typedef struct { - uint8_t js_x; - uint8_t js_y; - bool bt_c; - bool bt_z; - bool bt_push; - bool rev_has_state; - bool is_rev; - float vbat; +typedef struct +{ + uint8_t js_x; + uint8_t js_y; + bool bt_c; + bool bt_z; + bool bt_push; + bool rev_has_state; + bool is_rev; + float vbat; } mote_state; -typedef enum { - MOTE_PACKET_BATT_LEVEL = 0, - MOTE_PACKET_BUTTONS, - MOTE_PACKET_ALIVE, - MOTE_PACKET_FILL_RX_BUFFER, - MOTE_PACKET_FILL_RX_BUFFER_LONG, - MOTE_PACKET_PROCESS_RX_BUFFER, - MOTE_PACKET_PROCESS_SHORT_BUFFER, - MOTE_PACKET_PAIRING_INFO +typedef enum +{ + MOTE_PACKET_BATT_LEVEL = 0, + MOTE_PACKET_BUTTONS, + MOTE_PACKET_ALIVE, + MOTE_PACKET_FILL_RX_BUFFER, + MOTE_PACKET_FILL_RX_BUFFER_LONG, + MOTE_PACKET_PROCESS_RX_BUFFER, + MOTE_PACKET_PROCESS_SHORT_BUFFER, + MOTE_PACKET_PAIRING_INFO } MOTE_PACKET; -typedef struct { - float v_in; - float temp_mos; - float temp_mos_1; - float temp_mos_2; - float temp_mos_3; - float temp_motor; - float current_motor; - float current_in; - float id; - float iq; - float rpm; - float duty_now; - float amp_hours; - float amp_hours_charged; - float watt_hours; - float watt_hours_charged; - int tachometer; - int tachometer_abs; - float position; - mc_fault_code fault_code; - int vesc_id; - float vd; - float vq; +typedef struct +{ + float v_in; + float temp_mos; + float temp_mos_1; + float temp_mos_2; + float temp_mos_3; + float temp_motor; + float current_motor; + float current_in; + float id; + float iq; + float rpm; + float duty_now; + float amp_hours; + float amp_hours_charged; + float watt_hours; + float watt_hours_charged; + int tachometer; + int tachometer_abs; + float position; + mc_fault_code fault_code; + int vesc_id; + float vd; + float vq; } mc_values; -typedef enum { - NRF_PAIR_STARTED = 0, - NRF_PAIR_OK, - NRF_PAIR_FAIL +typedef enum +{ + NRF_PAIR_STARTED = 0, + NRF_PAIR_OK, + NRF_PAIR_FAIL } NRF_PAIR_RES; // Orientation data -typedef struct { - float q0; - float q1; - float q2; - float q3; - float integralFBx; - float integralFBy; - float integralFBz; - float accMagP; - int initialUpdateDone; +typedef struct +{ + float q0; + float q1; + float q2; + float q3; + float integralFBx; + float integralFBy; + float integralFBz; + float accMagP; + int initialUpdateDone; } ATTITUDE_INFO; // Custom EEPROM variables typedef union { - uint32_t as_u32; - int32_t as_i32; - float as_float; + uint32_t as_u32; + int32_t as_i32; + float as_float; } eeprom_var; -#define EEPROM_VARS_HW 64 -#define EEPROM_VARS_CUSTOM 64 - -typedef struct { - float ah_tot; - float ah_charge_tot; - float wh_tot; - float wh_charge_tot; - float current_tot; - float current_in_tot; - uint8_t num_vescs; +#define EEPROM_VARS_HW 64 +#define EEPROM_VARS_CUSTOM 64 + +typedef struct +{ + float ah_tot; + float ah_charge_tot; + float wh_tot; + float wh_charge_tot; + float current_tot; + float current_in_tot; + uint8_t num_vescs; } setup_values; -} -#endif /* DATATYPES_H_ */ +} // namespace vesc_driver +#endif // VESC_DRIVER__DATATYPES_HPP_ diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index fa1fc3c..aa47cc8 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -121,18 +121,18 @@ class VescPacketFWVersion : public VescPacket int fwMajor() const; int fwMinor() const; - std::string hwname() const; - const uint8_t* uuid() const; + std::string hwname() const; + const uint8_t * uuid() const; bool paired() const; uint8_t devVersion() const; - private: +private: int minor_; int major_; std::string hwname_; bool paired_; - uint8_t uuid_[12]; - uint8_t devVersion_; + uint8_t uuid_[12]; + uint8_t devVersion_; }; class VescPacketRequestFWVersion : public VescPacket @@ -148,35 +148,34 @@ class VescPacketValues : public VescPacket public: explicit VescPacketValues(std::shared_ptr raw); -double temp_fet() const; -double temp_motor() const; -double avg_motor_current() const; -double avg_input_current() const; -double avg_id() const; -double avg_iq() const ; -double duty_cycle_now() const; -double rpm() const; -double duty_now() const; -double v_in() const; -double amp_hours() const; -double amp_hours_charged() const; -double watt_hours() const; -double watt_hours_charged() const; -int32_t tachometer() const; -int32_t tachometer_abs() const; -int fault_code() const; -double pid_pos_now() const; -int32_t controller_id() const; - -double temp_mos1() const; -double temp_mos2() const; -double temp_mos3() const; -double avg_vd() const; -double avg_vq() const; - -int32_t numVescs() const; -double watt_battery_left() const; - + double temp_fet() const; + double temp_motor() const; + double avg_motor_current() const; + double avg_input_current() const; + double avg_id() const; + double avg_iq() const; + double duty_cycle_now() const; + double rpm() const; + double duty_now() const; + double v_in() const; + double amp_hours() const; + double amp_hours_charged() const; + double watt_hours() const; + double watt_hours_charged() const; + int32_t tachometer() const; + int32_t tachometer_abs() const; + int fault_code() const; + double pid_pos_now() const; + int32_t controller_id() const; + + double temp_mos1() const; + double temp_mos2() const; + double temp_mos3() const; + double avg_vd() const; + double avg_vq() const; + + int32_t numVescs() const; + double watt_battery_left() const; }; class VescPacketRequestValues : public VescPacket diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index c2135b5..d2ba4b3 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -162,54 +162,56 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa auto state_msg = VescStateStamped(); state_msg.header.stamp = now(); - + state_msg.state.voltage_input = values->v_in(); state_msg.state.current_motor = values->avg_motor_current(); state_msg.state.current_input = values->avg_input_current(); - state_msg.state.avg_id = values->avg_id(); - state_msg.state.avg_iq = values->avg_iq(); - state_msg.state.duty_cycle = values->duty_cycle_now(); - state_msg.state.speed = values->rpm(); - + state_msg.state.avg_id = values->avg_id(); + state_msg.state.avg_iq = values->avg_iq(); + state_msg.state.duty_cycle = values->duty_cycle_now(); + state_msg.state.speed = values->rpm(); + state_msg.state.charge_drawn = values->amp_hours(); state_msg.state.charge_regen = values->amp_hours_charged(); state_msg.state.energy_drawn = values->watt_hours(); state_msg.state.energy_regen = values->watt_hours_charged(); state_msg.state.displacement = values->tachometer(); state_msg.state.distance_traveled = values->tachometer_abs(); - state_msg.state.fault_code = values->fault_code(); + state_msg.state.fault_code = values->fault_code(); - state_msg.state.pid_pos_now = values->pid_pos_now(); - state_msg.state.controller_id = values->controller_id(); + state_msg.state.pid_pos_now = values->pid_pos_now(); + state_msg.state.controller_id = values->controller_id(); - state_msg.state.ntc_temp_mos1 = values->temp_mos1(); - state_msg.state.ntc_temp_mos2 = values->temp_mos2(); - state_msg.state.ntc_temp_mos3 = values->temp_mos3(); - state_msg.state.avg_vd = values->avg_vd(); - state_msg.state.avg_vq = values->avg_vq(); + state_msg.state.ntc_temp_mos1 = values->temp_mos1(); + state_msg.state.ntc_temp_mos2 = values->temp_mos2(); + state_msg.state.ntc_temp_mos3 = values->temp_mos3(); + state_msg.state.avg_vd = values->avg_vd(); + state_msg.state.avg_vq = values->avg_vq(); state_pub_->publish(state_msg); } else if (packet->name() == "FWVersion") { - std::shared_ptr fw_version = + std::shared_ptr fw_version = std::dynamic_pointer_cast(packet); // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); RCLCPP_INFO( - get_logger(), - "-=%s=- hardware paired %d", - fw_version->hwname().c_str(), - fw_version->paired() - ); + get_logger(), + "-=%s=- hardware paired %d", + fw_version->hwname().c_str(), + fw_version->paired() + ); } - - auto& clk = *this->get_clock(); + + auto & clk = *this->get_clock(); RCLCPP_INFO_THROTTLE( - get_logger(), - clk, - 5000, - "%s packet received",packet->name().c_str()); + get_logger(), + clk, + 5000, + "%s packet received", + packet->name().c_str() + ); } void VescDriver::vescErrorCallback(const std::string & error) diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 7ebf0cb..1cb9980 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -34,10 +34,11 @@ #include #include #include +#include #include "vesc_driver/datatypes.hpp" #include "vesc_driver/vesc_packet_factory.hpp" -#include + namespace vesc_driver { @@ -101,31 +102,22 @@ VescPacket::VescPacket(const std::string & name, std::shared_ptr raw) VescPacketFWVersion::VescPacketFWVersion(std::shared_ptr raw) : VescPacket("FWVersion", raw) { - major_ = *(payload_.first + 1); - minor_ = *(payload_.first + 2); - int j=0; - - - for(int i=0;*(payload_.first+3+i)!=0x00;i++){ - j = i; - hwname_ += *(payload_.first+3+i); - } + minor_ = *(payload_.first + 2); + int j = 0; - j++; - - - - - for(int u=0;u<12;u++){ - uuid_[u] = *(payload_.first+3+j+1+u); - + for (int i = 0; *(payload_.first + 3 + i) != 0x00; i++) { + j = i; + hwname_ += *(payload_.first + 3 + i); } - + j++; - paired_ = *(payload_.first+3+j+1+12); - devVersion_ = *(payload_.first+3+j+2+12+1); + for (int u = 0; u < 12; u++) { + uuid_[u] = *(payload_.first + 3 + j + 1 + u); + } + paired_ = *(payload_.first + 3 + j + 1 + 12); + devVersion_ = *(payload_.first + 3 + j + 2 + 12 + 1); } int VescPacketFWVersion::fwMajor() const @@ -138,22 +130,21 @@ int VescPacketFWVersion::fwMinor() const return minor_; } -std::string VescPacketFWVersion::hwname() const +std::string VescPacketFWVersion::hwname() const { - - return hwname_.c_str(); + return hwname_.c_str(); } -const uint8_t* VescPacketFWVersion::uuid() const +const uint8_t * VescPacketFWVersion::uuid() const { return uuid_; } -bool VescPacketFWVersion::paired() const +bool VescPacketFWVersion::paired() const { - return paired_; + return paired_; } -uint8_t VescPacketFWVersion::devVersion() const +uint8_t VescPacketFWVersion::devVersion() const { - return devVersion_; + return devVersion_; } REGISTER_PACKET_TYPE(COMM_FW_VERSION, VescPacketFWVersion) @@ -173,141 +164,164 @@ VescPacketValues::VescPacketValues(std::shared_ptr raw) : VescPacket("Values", raw) { } -double VescPacketValues::temp_fet() const{ - int16_t v = static_cast((static_cast(*(payload_.first + 1)) << 8) + - static_cast(*(payload_.first + 2))); +double VescPacketValues::temp_fet() const +{ + int16_t v = static_cast( + (static_cast(*(payload_.first + 1)) << 8) + + (static_cast(*(payload_.first + 2)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::temp_motor() const{ - int16_t v = static_cast((static_cast(*(payload_.first + 3)) << 8) + - static_cast(*(payload_.first + 4))); +double VescPacketValues::temp_motor() const +{ + int16_t v = static_cast( + (static_cast(*(payload_.first + 3)) << 8) + + (static_cast(*(payload_.first + 4)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::avg_motor_current() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 5)) << 24) + - (static_cast(*(payload_.first + 6)) << 16) + - (static_cast(*(payload_.first + 7)) << 8) + - static_cast(*(payload_.first + 8)) - ); +double VescPacketValues::avg_motor_current() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 5)) << 24) + + (static_cast(*(payload_.first + 6)) << 16) + + (static_cast(*(payload_.first + 7)) << 8) + + (static_cast(*(payload_.first + 8)) ) + ); return static_cast(v) / 100.0; - } -double VescPacketValues::avg_input_current() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 9)) << 24) + - (static_cast(*(payload_.first + 10)) << 16) + - (static_cast(*(payload_.first + 11)) << 8) + - static_cast(*(payload_.first + 12)) - ); +double VescPacketValues::avg_input_current() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 9)) << 24) + + (static_cast(*(payload_.first + 10)) << 16) + + (static_cast(*(payload_.first + 11)) << 8) + + (static_cast(*(payload_.first + 12)) ) + ); return static_cast(v) / 100.0; - } -double VescPacketValues::avg_id() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 13)) << 24) + - (static_cast(*(payload_.first + 14)) << 16) + - (static_cast(*(payload_.first + 15)) << 8) + - static_cast(*(payload_.first + 16)) - ); +double VescPacketValues::avg_id() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 13)) << 24) + + (static_cast(*(payload_.first + 14)) << 16) + + (static_cast(*(payload_.first + 15)) << 8) + + (static_cast(*(payload_.first + 16)) ) + ); return static_cast(v) / 100.0; - } -double VescPacketValues::avg_iq() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 17)) << 24) + - (static_cast(*(payload_.first + 18)) << 16) + - (static_cast(*(payload_.first + 19)) << 8) + - static_cast(*(payload_.first + 20)) - ); +double VescPacketValues::avg_iq() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 17)) << 24) + + (static_cast(*(payload_.first + 18)) << 16) + + (static_cast(*(payload_.first + 19)) << 8) + + (static_cast(*(payload_.first + 20)) ) + ); return static_cast(v) / 100.0; - } double VescPacketValues::duty_cycle_now() const { - int16_t v = static_cast((static_cast(*(payload_.first + 21)) << 8) + - static_cast(*(payload_.first + 22))); + int16_t v = static_cast( + (static_cast(*(payload_.first + 21)) << 8) + + (static_cast(*(payload_.first + 22)) ) + ); return static_cast(v) / 1000.0; } -double VescPacketValues::rpm() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 23)) << 24) + - (static_cast(*(payload_.first + 24)) << 16) + - (static_cast(*(payload_.first + 25)) << 8) + - static_cast(*(payload_.first + 26)) - ); +double VescPacketValues::rpm() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 23)) << 24) + + (static_cast(*(payload_.first + 24)) << 16) + + (static_cast(*(payload_.first + 25)) << 8) + + (static_cast(*(payload_.first + 26)) ) + ); return static_cast(v) / 1.0; - } double VescPacketValues::v_in() const { - int16_t v = static_cast((static_cast(*(payload_.first + 27)) << 8) + - static_cast(*(payload_.first + 28))); - return static_cast(v) /10.0; + int16_t v = static_cast( + (static_cast(*(payload_.first + 27)) << 8) + + (static_cast(*(payload_.first + 28)) ) + ); + return static_cast(v) / 10.0; } -double VescPacketValues::amp_hours() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 29)) << 24) + - (static_cast(*(payload_.first + 30)) << 16) + - (static_cast(*(payload_.first + 31)) << 8) + - static_cast(*(payload_.first + 32)) - ); +double VescPacketValues::amp_hours() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 29)) << 24) + + (static_cast(*(payload_.first + 30)) << 16) + + (static_cast(*(payload_.first + 31)) << 8) + + (static_cast(*(payload_.first + 32)) ) + ); return static_cast(v) / 1e4; - } -double VescPacketValues::amp_hours_charged() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 33)) << 24) + - (static_cast(*(payload_.first + 34)) << 16) + - (static_cast(*(payload_.first + 35)) << 8) + - static_cast(*(payload_.first + 36)) - ); +double VescPacketValues::amp_hours_charged() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 33)) << 24) + + (static_cast(*(payload_.first + 34)) << 16) + + (static_cast(*(payload_.first + 35)) << 8) + + (static_cast(*(payload_.first + 36)) ) + ); return static_cast(v) / 1e4; - } -double VescPacketValues::watt_hours() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 37)) << 24) + - (static_cast(*(payload_.first + 38)) << 16) + - (static_cast(*(payload_.first + 39)) << 8) + - static_cast(*(payload_.first + 40)) - ); +double VescPacketValues::watt_hours() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 37)) << 24) + + (static_cast(*(payload_.first + 38)) << 16) + + (static_cast(*(payload_.first + 39)) << 8) + + (static_cast(*(payload_.first + 40)) ) + ); return static_cast(v) / 1e4; - } -double VescPacketValues::watt_hours_charged() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 41)) << 24) + - (static_cast(*(payload_.first + 42)) << 16) + - (static_cast(*(payload_.first + 43)) << 8) + - static_cast(*(payload_.first + 44)) - ); +double VescPacketValues::watt_hours_charged() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 41)) << 24) + + (static_cast(*(payload_.first + 42)) << 16) + + (static_cast(*(payload_.first + 43)) << 8) + + (static_cast(*(payload_.first + 44)) ) + ); return static_cast(v) / 1e4; - } int32_t VescPacketValues::tachometer() const { - int32_t v = static_cast((static_cast(*(payload_.first + 45)) << 24) + - (static_cast(*(payload_.first + 46)) << 16) + - (static_cast(*(payload_.first + 47)) << 8) + - static_cast(*(payload_.first + 48))); + int32_t v = static_cast( + (static_cast(*(payload_.first + 45)) << 24) + + (static_cast(*(payload_.first + 46)) << 16) + + (static_cast(*(payload_.first + 47)) << 8) + + (static_cast(*(payload_.first + 48)) ) + ); return static_cast(v); } int32_t VescPacketValues::tachometer_abs() const { - int32_t v = static_cast((static_cast(*(payload_.first + 49)) << 24) + - (static_cast(*(payload_.first + 50)) << 16) + - (static_cast(*(payload_.first + 51)) << 8) + - static_cast(*(payload_.first + 52))); + int32_t v = static_cast( + (static_cast(*(payload_.first + 49)) << 24) + + (static_cast(*(payload_.first + 50)) << 16) + + (static_cast(*(payload_.first + 51)) << 8) + + (static_cast(*(payload_.first + 52)) ) + ); return static_cast(v); } @@ -319,67 +333,70 @@ int VescPacketValues::fault_code() const double VescPacketValues::pid_pos_now() const { - int32_t v = static_cast((static_cast(*(payload_.first + 54)) << 24) + - (static_cast(*(payload_.first + 55)) << 16) + - (static_cast(*(payload_.first + 56)) << 8) + - static_cast(*(payload_.first + 57)) - ); + int32_t v = static_cast( + (static_cast(*(payload_.first + 54)) << 24) + + (static_cast(*(payload_.first + 55)) << 16) + + (static_cast(*(payload_.first + 56)) << 8) + + (static_cast(*(payload_.first + 57)) ) + ); return static_cast(v) / 1e6; - } int32_t VescPacketValues::controller_id() const { - //int32_t v = static_cast((static_cast(*(payload_.first + 58)) << 24) + - // (static_cast(*(payload_.first + 59)) << 16) + - // (static_cast(*(payload_.first + 60)) << 8) + - // static_cast(*(payload_.first + 61))); -// - int32_t v = static_cast((static_cast(*(payload_.first + 58) ))); + int32_t v = static_cast((static_cast(*(payload_.first + 58) ))); return static_cast(v); } double VescPacketValues::temp_mos1() const { - int16_t v = static_cast((static_cast(*(payload_.first + 59)) << 8) + - static_cast(*(payload_.first + 60))); + 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 + 61)) << 8) + - static_cast(*(payload_.first + 62))); + 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))); + int16_t v = static_cast( + (static_cast(*(payload_.first + 63)) << 8) + + (static_cast(*(payload_.first + 64)) ) + ); return static_cast(v) / 10.0; } -double VescPacketValues::avg_vd() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 65)) << 24) + - (static_cast(*(payload_.first + 66)) << 16) + - (static_cast(*(payload_.first + 67)) << 8) + - static_cast(*(payload_.first + 68)) - ); +double VescPacketValues::avg_vd() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 65)) << 24) + + (static_cast(*(payload_.first + 66)) << 16) + + (static_cast(*(payload_.first + 67)) << 8) + + (static_cast(*(payload_.first + 68)) ) + ); return static_cast(v) / 1e3; - } -double VescPacketValues::avg_vq() const{ - int32_t v = static_cast((static_cast(*(payload_.first + 69)) << 24) + - (static_cast(*(payload_.first + 70)) << 16) + - (static_cast(*(payload_.first + 71)) << 8) + - static_cast(*(payload_.first + 72)) - ); +double VescPacketValues::avg_vq() const +{ + int32_t v = static_cast( + (static_cast(*(payload_.first + 69)) << 24) + + (static_cast(*(payload_.first + 70)) << 16) + + (static_cast(*(payload_.first + 71)) << 8) + + (static_cast(*(payload_.first + 72)) ) + ); return static_cast(v) / 1e3; - } From e4de5f7c09c0b3c8f089fede2fbbb49550605a44 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Thu, 3 Jun 2021 21:14:40 +0200 Subject: [PATCH 40/72] removed old methods --- vesc_driver/include/vesc_driver/vesc_packet.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index aa47cc8..74ccd72 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -173,9 +173,6 @@ class VescPacketValues : public VescPacket double temp_mos3() const; double avg_vd() const; double avg_vq() const; - - int32_t numVescs() const; - double watt_battery_left() const; }; class VescPacketRequestValues : public VescPacket From 039fcefd845857a65089b22673161a6f0cd02ed3 Mon Sep 17 00:00:00 2001 From: Haoru Date: Mon, 17 May 2021 02:06:51 +0000 Subject: [PATCH 41/72] working ros2 driver --- README.md | 2 +- vesc_driver/CMakeLists.txt | 19 +- vesc_driver/src/vesc_interface_old.cpp | 293 +++++++++++++++++++++++++ 3 files changed, 304 insertions(+), 10 deletions(-) create mode 100644 vesc_driver/src/vesc_interface_old.cpp diff --git a/README.md b/README.md index 575fa6e..e96dbd2 100644 --- a/README.md +++ b/README.md @@ -34,4 +34,4 @@ Potential Improvements: 3. Plug in the VESC with a USB cable. 4. Modify `vesc/vesc_driver/params/vesc_config.yaml` to reflect any changes. 5. Build the package `colcon build` -6. `ros2 launch vesc_driver vesc_driver_node.launch.py` \ No newline at end of file +6. `ros2 launch vesc_driver vesc_driver_node.launch.py` diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index c6431ce..8361e43 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,16 +1,17 @@ cmake_minimum_required(VERSION 3.5) project(vesc_driver) -# Set minimum C++ standard to C++14 -if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++14") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") -elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++14") - set(CMAKE_CXX_STANDARD 14) -endif() - +# Set minimum C++ standard to C++17 +#if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") +# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") +#elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") +# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") +# set(CMAKE_CXX_STANDARD 17) +#endif() +set(CMAKE_CXX_STANDARD 17) find_package(ament_cmake_auto REQUIRED) +find_package(serial_driver REQUIRED) ament_auto_find_build_dependencies() find_package(Threads) diff --git a/vesc_driver/src/vesc_interface_old.cpp b/vesc_driver/src/vesc_interface_old.cpp new file mode 100644 index 0000000..0b0c20d --- /dev/null +++ b/vesc_driver/src/vesc_interface_old.cpp @@ -0,0 +1,293 @@ +// Copyright 2020 F1TENTH Foundation +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// -*- mode:c++; fill-column: 100; -*- + +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet_factory.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vesc_driver +{ + +class VescInterface::Impl +{ +public: + Impl() + : io_service_(), + serial_port_(io_service_) + {} + + void rxThread(); + + std::unique_ptr rx_thread_; + bool rx_thread_run_; + PacketHandlerFunction packet_handler_; + ErrorHandlerFunction error_handler_; + boost::asio::io_service io_service_; + boost::asio::serial_port serial_port_; + std::mutex serial_mutex_; +}; + +void VescInterface::Impl::rxThread() +{ + Buffer buffer; + buffer.reserve(4096); + + while (rx_thread_run_) { + int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + if (!buffer.empty()) { + // search buffer for valid packet(s) + Buffer::iterator iter(buffer.begin()); + Buffer::iterator iter_begin(buffer.begin()); + while (iter != buffer.end()) { + // check if valid start-of-frame character + if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || + VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) + { + // good start, now attempt to create packet + std::string error; + VescPacketConstPtr packet = + VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); + if (packet) { + // good packet, check if we skipped any data + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << + std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); + } + // call packet handler + packet_handler_(packet); + // update state + iter = iter + packet->frame().size(); + iter_begin = iter; + // continue to look for another frame in buffer + continue; + } else if (bytes_needed > 0) { + // need more data, break out of while loop + break; // for (iter_sof... + } else { + // else, this was not a packet, move on to next byte + error_handler_(error); + } + } + + iter++; + } + + // if iter is at the end of the buffer, more bytes are needed + if (iter == buffer.end()) { + bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; + } + + // erase "used" buffer + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); + } + buffer.erase(buffer.begin(), iter); + } + + // attempt to read at least bytes_needed bytes from the serial port + int bytes_to_read = std::max(bytes_needed, 4096); + + { + std::lock_guard lock(serial_mutex_); + + const size_t bytes_read = boost::asio::read( + serial_port_, + boost::asio::buffer(buffer, buffer.size()), + boost::asio::transfer_exactly(bytes_to_read)); + + if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { + error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); + } + } + + // Only attempt to read every 10 ms + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +VescInterface::VescInterface( + const std::string & port, + const PacketHandlerFunction & packet_handler, + const ErrorHandlerFunction & error_handler) +: impl_(new Impl()) +{ + setPacketHandler(packet_handler); + setErrorHandler(error_handler); + // attempt to conect if the port is specified + if (!port.empty()) { + connect(port); + } +} + +VescInterface::~VescInterface() +{ + disconnect(); +} + +void VescInterface::setPacketHandler(const PacketHandlerFunction & handler) +{ + // todo - definately need mutex + impl_->packet_handler_ = handler; +} + +void VescInterface::setErrorHandler(const ErrorHandlerFunction & handler) +{ + // todo - definately need mutex + impl_->error_handler_ = handler; +} + +void VescInterface::connect(const std::string & port) +{ + // todo - mutex? + + if (isConnected()) { + throw SerialException("Already connected to serial port."); + } + + // connect to serial port + try { + impl_->serial_port_.open(port); + impl_->serial_port_.set_option(boost::asio::serial_port_base::baud_rate(115200)); + impl_->serial_port_.set_option( + boost::asio::serial_port::flow_control( + boost::asio::serial_port::flow_control::none)); + impl_->serial_port_.set_option( + boost::asio::serial_port::parity( + boost::asio::serial_port::parity::none)); + impl_->serial_port_.set_option( + boost::asio::serial_port::stop_bits( + boost::asio::serial_port::stop_bits::one)); + } catch (const std::exception & e) { + std::stringstream ss; + ss << "Failed to open the serial port to the VESC. " << e.what(); + throw SerialException(ss.str().c_str()); + } + + // start up a monitoring thread + impl_->rx_thread_run_ = true; + impl_->rx_thread_ = std::unique_ptr( + new std::thread( + &VescInterface::Impl::rxThread, impl_.get())); +} + +void VescInterface::disconnect() +{ + // todo - mutex? + + if (isConnected()) { + // bring down read thread + impl_->rx_thread_run_ = false; + impl_->rx_thread_->join(); + + std::lock_guard lock(impl_->serial_mutex_); + impl_->serial_port_.close(); + } +} + +bool VescInterface::isConnected() const +{ + std::lock_guard lock(impl_->serial_mutex_); + return impl_->serial_port_.is_open(); +} + +void VescInterface::send(const VescPacket & packet) +{ + try { + std::lock_guard lock(impl_->serial_mutex_); + size_t written = impl_->serial_port_.write_some( + boost::asio::buffer(packet.frame())); + if (written != packet.frame().size()) { + std::stringstream ss; + ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; + throw SerialException(ss.str().c_str()); + } + } catch (const std::exception & e) { + std::stringstream ss; + ss << "Failed to open the serial port to the VESC. " << e.what(); + throw SerialException(ss.str().c_str()); + } +} + +void VescInterface::requestFWVersion() +{ + send(VescPacketRequestFWVersion()); +} + +void VescInterface::requestState() +{ + send(VescPacketRequestValues()); +} + +void VescInterface::setDutyCycle(double duty_cycle) +{ + send(VescPacketSetDuty(duty_cycle)); +} + +void VescInterface::setCurrent(double current) +{ + send(VescPacketSetCurrent(current)); +} + +void VescInterface::setBrake(double brake) +{ + send(VescPacketSetCurrentBrake(brake)); +} + +void VescInterface::setSpeed(double speed) +{ + send(VescPacketSetRPM(speed)); +} + +void VescInterface::setPosition(double position) +{ + send(VescPacketSetPos(position)); +} + +void VescInterface::setServo(double servo) +{ + send(VescPacketSetServoPos(servo)); +} + +} // namespace vesc_driver From b9741d0399f1349df608c08b36b5476e8f517b8b Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Fri, 6 Aug 2021 10:17:38 -1000 Subject: [PATCH 42/72] rebased to upstream --- vesc_driver/src/vesc_interface_old.cpp | 293 ------------------------- 1 file changed, 293 deletions(-) delete mode 100644 vesc_driver/src/vesc_interface_old.cpp diff --git a/vesc_driver/src/vesc_interface_old.cpp b/vesc_driver/src/vesc_interface_old.cpp deleted file mode 100644 index 0b0c20d..0000000 --- a/vesc_driver/src/vesc_interface_old.cpp +++ /dev/null @@ -1,293 +0,0 @@ -// Copyright 2020 F1TENTH Foundation -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// -*- mode:c++; fill-column: 100; -*- - -#include "vesc_driver/vesc_interface.hpp" -#include "vesc_driver/vesc_packet_factory.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace vesc_driver -{ - -class VescInterface::Impl -{ -public: - Impl() - : io_service_(), - serial_port_(io_service_) - {} - - void rxThread(); - - std::unique_ptr rx_thread_; - bool rx_thread_run_; - PacketHandlerFunction packet_handler_; - ErrorHandlerFunction error_handler_; - boost::asio::io_service io_service_; - boost::asio::serial_port serial_port_; - std::mutex serial_mutex_; -}; - -void VescInterface::Impl::rxThread() -{ - Buffer buffer; - buffer.reserve(4096); - - while (rx_thread_run_) { - int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - if (!buffer.empty()) { - // search buffer for valid packet(s) - Buffer::iterator iter(buffer.begin()); - Buffer::iterator iter_begin(buffer.begin()); - while (iter != buffer.end()) { - // check if valid start-of-frame character - if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || - VescFrame::VESC_SOF_VAL_LARGE_FRAME == *iter) - { - // good start, now attempt to create packet - std::string error; - VescPacketConstPtr packet = - VescPacketFactory::createPacket(iter, buffer.end(), &bytes_needed, &error); - if (packet) { - // good packet, check if we skipped any data - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << - std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } - // call packet handler - packet_handler_(packet); - // update state - iter = iter + packet->frame().size(); - iter_begin = iter; - // continue to look for another frame in buffer - continue; - } else if (bytes_needed > 0) { - // need more data, break out of while loop - break; // for (iter_sof... - } else { - // else, this was not a packet, move on to next byte - error_handler_(error); - } - } - - iter++; - } - - // if iter is at the end of the buffer, more bytes are needed - if (iter == buffer.end()) { - bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; - } - - // erase "used" buffer - if (std::distance(iter_begin, iter) > 0) { - std::ostringstream ss; - ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; - error_handler_(ss.str()); - } - buffer.erase(buffer.begin(), iter); - } - - // attempt to read at least bytes_needed bytes from the serial port - int bytes_to_read = std::max(bytes_needed, 4096); - - { - std::lock_guard lock(serial_mutex_); - - const size_t bytes_read = boost::asio::read( - serial_port_, - boost::asio::buffer(buffer, buffer.size()), - boost::asio::transfer_exactly(bytes_to_read)); - - if (bytes_needed > 0 && 0 == bytes_read && !buffer.empty()) { - error_handler_("Possibly out-of-sync with VESC, read timout in the middle of a frame."); - } - } - - // Only attempt to read every 10 ms - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } -} - -VescInterface::VescInterface( - const std::string & port, - const PacketHandlerFunction & packet_handler, - const ErrorHandlerFunction & error_handler) -: impl_(new Impl()) -{ - setPacketHandler(packet_handler); - setErrorHandler(error_handler); - // attempt to conect if the port is specified - if (!port.empty()) { - connect(port); - } -} - -VescInterface::~VescInterface() -{ - disconnect(); -} - -void VescInterface::setPacketHandler(const PacketHandlerFunction & handler) -{ - // todo - definately need mutex - impl_->packet_handler_ = handler; -} - -void VescInterface::setErrorHandler(const ErrorHandlerFunction & handler) -{ - // todo - definately need mutex - impl_->error_handler_ = handler; -} - -void VescInterface::connect(const std::string & port) -{ - // todo - mutex? - - if (isConnected()) { - throw SerialException("Already connected to serial port."); - } - - // connect to serial port - try { - impl_->serial_port_.open(port); - impl_->serial_port_.set_option(boost::asio::serial_port_base::baud_rate(115200)); - impl_->serial_port_.set_option( - boost::asio::serial_port::flow_control( - boost::asio::serial_port::flow_control::none)); - impl_->serial_port_.set_option( - boost::asio::serial_port::parity( - boost::asio::serial_port::parity::none)); - impl_->serial_port_.set_option( - boost::asio::serial_port::stop_bits( - boost::asio::serial_port::stop_bits::one)); - } catch (const std::exception & e) { - std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); - throw SerialException(ss.str().c_str()); - } - - // start up a monitoring thread - impl_->rx_thread_run_ = true; - impl_->rx_thread_ = std::unique_ptr( - new std::thread( - &VescInterface::Impl::rxThread, impl_.get())); -} - -void VescInterface::disconnect() -{ - // todo - mutex? - - if (isConnected()) { - // bring down read thread - impl_->rx_thread_run_ = false; - impl_->rx_thread_->join(); - - std::lock_guard lock(impl_->serial_mutex_); - impl_->serial_port_.close(); - } -} - -bool VescInterface::isConnected() const -{ - std::lock_guard lock(impl_->serial_mutex_); - return impl_->serial_port_.is_open(); -} - -void VescInterface::send(const VescPacket & packet) -{ - try { - std::lock_guard lock(impl_->serial_mutex_); - size_t written = impl_->serial_port_.write_some( - boost::asio::buffer(packet.frame())); - if (written != packet.frame().size()) { - std::stringstream ss; - ss << "Wrote " << written << " bytes, expected " << packet.frame().size() << "."; - throw SerialException(ss.str().c_str()); - } - } catch (const std::exception & e) { - std::stringstream ss; - ss << "Failed to open the serial port to the VESC. " << e.what(); - throw SerialException(ss.str().c_str()); - } -} - -void VescInterface::requestFWVersion() -{ - send(VescPacketRequestFWVersion()); -} - -void VescInterface::requestState() -{ - send(VescPacketRequestValues()); -} - -void VescInterface::setDutyCycle(double duty_cycle) -{ - send(VescPacketSetDuty(duty_cycle)); -} - -void VescInterface::setCurrent(double current) -{ - send(VescPacketSetCurrent(current)); -} - -void VescInterface::setBrake(double brake) -{ - send(VescPacketSetCurrentBrake(brake)); -} - -void VescInterface::setSpeed(double speed) -{ - send(VescPacketSetRPM(speed)); -} - -void VescInterface::setPosition(double position) -{ - send(VescPacketSetPos(position)); -} - -void VescInterface::setServo(double servo) -{ - send(VescPacketSetServoPos(servo)); -} - -} // namespace vesc_driver From ab8e179ca004a353d3ffe073427db5b4ec611540 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Tue, 10 Aug 2021 15:39:41 -0400 Subject: [PATCH 43/72] node naving convention; removed unused variable --- vesc_driver/CMakeLists.txt | 2 +- vesc_driver/src/vesc_interface.cpp | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index c6431ce..1f0f4ba 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -31,7 +31,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} PLUGIN vesc_driver::VescDriver - EXECUTABLE vesc_driver_node + EXECUTABLE ${PROJECT_NAME}_node ) ############# diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index c73ce95..d32b819 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -95,7 +95,6 @@ void VescInterface::Impl::packet_creation_thread() if (!buffer_.empty()) { // search buffer for valid packet(s) auto iter = buffer_.begin(); - auto iter_begin = buffer_.begin(); while (iter != buffer_.end()) { // check if valid start-of-frame character @@ -111,7 +110,6 @@ void VescInterface::Impl::packet_creation_thread() packet_handler_(packet); // update state iter = iter + packet->frame().size(); - iter_begin = iter; // continue to look for another frame in buffer continue; } else if (bytes_needed > 0) { From 84f8b63399c5b818a45b58087a08b61ca6440ed9 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Tue, 10 Aug 2021 15:54:35 -0400 Subject: [PATCH 44/72] update cmake --- vesc_driver/CMakeLists.txt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 23abfb7..2a141b9 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -1,15 +1,15 @@ cmake_minimum_required(VERSION 3.5) project(vesc_driver) -# Set minimum C++ standard to C++17 -#if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") -# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") -#elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") -# message(STATUS "Changing CXX_STANDARD from C++98 to C++17") -# set(CMAKE_CXX_STANDARD 17) -#endif() -set(CMAKE_CXX_STANDARD 17) +# Set minimum C++ standard to C++14 +if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") +elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_STANDARD 14) +endif() + find_package(ament_cmake_auto REQUIRED) find_package(serial_driver REQUIRED) ament_auto_find_build_dependencies() From eb0ef55b7f4a1a6aaa2e1c0cf10d9746a693729d Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Tue, 10 Aug 2021 16:08:54 -0400 Subject: [PATCH 45/72] update main branch changes --- vesc_driver/src/vesc_driver.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index d2ba4b3..8c6e272 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -65,12 +65,7 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) fw_version_minor_(-1) { // get vesc serial port address - /* default to /tmp/ttyV0 for debug purpose - * use : - * socat /dev/ttyACM0,raw,echo=0 SYSTEM:'tee in.txt |socat - "PTY,link=/tmp/ttyV0,raw,echo=0,waitslave" |tee out.txt' - * to sniff comunication data to and from vesc - */ - std::string port = declare_parameter("port", "/tmp/ttyV0"); + std::string port = declare_parameter("port", ""); // attempt to connect to the serial port try { @@ -196,7 +191,7 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); - RCLCPP_INFO( + RCLCPP_DEBUG( get_logger(), "-=%s=- hardware paired %d", fw_version->hwname().c_str(), @@ -205,7 +200,7 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa } auto & clk = *this->get_clock(); - RCLCPP_INFO_THROTTLE( + RCLCPP_DEBUG_THROTTLE( get_logger(), clk, 5000, From 8c9bb0a6692b19d085d1cd8b88f99cdf92c59f2a Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Tue, 10 Aug 2021 16:35:42 -0400 Subject: [PATCH 46/72] removed debug codes from 654ba11 --- vesc_driver/src/vesc_driver.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 8c6e272..5db7395 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -190,23 +190,7 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); - - RCLCPP_DEBUG( - get_logger(), - "-=%s=- hardware paired %d", - fw_version->hwname().c_str(), - fw_version->paired() - ); } - - auto & clk = *this->get_clock(); - RCLCPP_DEBUG_THROTTLE( - get_logger(), - clk, - 5000, - "%s packet received", - packet->name().c_str() - ); } void VescDriver::vescErrorCallback(const std::string & error) From 4854265c9fdfd01e4b019fabbf31777d2b9fcb87 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Wed, 11 Aug 2021 21:04:48 -0400 Subject: [PATCH 47/72] remove serial driver from cmake --- vesc_driver/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 2a141b9..9909739 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -11,7 +11,6 @@ elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") endif() find_package(ament_cmake_auto REQUIRED) -find_package(serial_driver REQUIRED) ament_auto_find_build_dependencies() find_package(Threads) From 2c9dd35fdb4c2c5c41672f01c83a2758ec4c0a01 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Mon, 8 Nov 2021 10:42:48 -0600 Subject: [PATCH 48/72] Fix linter errors --- vesc_driver/CMakeLists.txt | 8 ++--- .../include/vesc_driver/vesc_driver.hpp | 5 +-- vesc_driver/launch/vesc_driver_node.launch.py | 31 ++++++++++++++-- vesc_driver/src/vesc_interface.cpp | 36 +++++++++---------- 4 files changed, 53 insertions(+), 27 deletions(-) diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 9909739..1f0f4ba 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -3,11 +3,11 @@ project(vesc_driver) # Set minimum C++ standard to C++14 if(NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}") - message(STATUS "Changing CXX_STANDARD from C++98 to C++14") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") elseif("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98") - message(STATUS "Changing CXX_STANDARD from C++98 to C++14") - set(CMAKE_CXX_STANDARD 14) + message(STATUS "Changing CXX_STANDARD from C++98 to C++14") + set(CMAKE_CXX_STANDARD 14) endif() find_package(ament_cmake_auto REQUIRED) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index f879f72..9377c36 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -36,9 +36,9 @@ #include #include +#include #include #include -#include #include "vesc_driver/vesc_interface.hpp" #include "vesc_driver/vesc_packet.hpp" @@ -69,7 +69,8 @@ class VescDriver rclcpp::Node * node_ptr, const std::string & str, const std::experimental::optional & min_lower = std::experimental::optional(), - const std::experimental::optional & max_upper = std::experimental::optional()); + const std::experimental::optional & max_upper = + std::experimental::optional()); double clip(double value); rclcpp::Node * node_ptr; rclcpp::Logger logger; diff --git a/vesc_driver/launch/vesc_driver_node.launch.py b/vesc_driver/launch/vesc_driver_node.launch.py index ffd83aa..005d68b 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.py +++ b/vesc_driver/launch/vesc_driver_node.launch.py @@ -1,3 +1,31 @@ +# Copyright 2020 F1TENTH Foundation +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + from launch import LaunchDescription from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node @@ -16,8 +44,7 @@ def generate_launch_description(): package='vesc_driver', executable='vesc_driver_node', name='vesc_driver_node', - parameters= [vesc_config] + parameters=[vesc_config] ), ]) - diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index d32b819..9ccf4d2 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -29,9 +29,6 @@ // -*- mode:c++; fill-column: 100; -*- #include "vesc_driver/vesc_interface.hpp" -#include "vesc_driver/vesc_packet_factory.hpp" -#include "serial_driver/serial_driver.hpp" - #include #include @@ -42,6 +39,10 @@ #include #include #include +#include + +#include "vesc_driver/vesc_packet_factory.hpp" +#include "serial_driver/serial_driver.hpp" namespace vesc_driver { @@ -51,7 +52,7 @@ class VescInterface::Impl public: Impl() : owned_ctx{new IoContext(2)}, - serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx)} + serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx)} {} void serial_receive_callback(const std::vector & buffer); @@ -68,19 +69,21 @@ class VescInterface::Impl std::string device_name_; std::unique_ptr owned_ctx{}; std::unique_ptr serial_driver_; - - ~Impl(){ + + ~Impl() + { if (owned_ctx) { owned_ctx->waitForExit(); + } } - } + private: std::vector buffer_; }; void VescInterface::Impl::serial_receive_callback(const std::vector & buffer) { - buffer_mutex_.lock(); // wait untill the current buffer read finishes + buffer_mutex_.lock(); // wait untill the current buffer read finishes buffer_.reserve(buffer_.size() + buffer.size()); buffer_.insert(buffer_.end(), buffer.begin(), buffer.end()); buffer_mutex_.unlock(); @@ -88,15 +91,13 @@ void VescInterface::Impl::serial_receive_callback(const std::vector & b void VescInterface::Impl::packet_creation_thread() { - while (packet_thread_run_) - { + while (packet_thread_run_) { buffer_mutex_.lock(); int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; if (!buffer_.empty()) { // search buffer for valid packet(s) auto iter = buffer_.begin(); - while (iter != buffer_.end()) - { + 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) @@ -142,14 +143,14 @@ void VescInterface::Impl::connect(const std::string & port) auto fc = drivers::serial_driver::FlowControl::HARDWARE; auto pt = drivers::serial_driver::Parity::NONE; auto sb = drivers::serial_driver::StopBits::ONE; - device_config_ = std::make_unique(baud_rate, fc, pt, sb); + device_config_ = + std::make_unique(baud_rate, fc, pt, sb); serial_driver_->init_port(port, *device_config_); if (!serial_driver_->port()->is_open()) { serial_driver_->port()->open(); serial_driver_->port()->async_receive( std::bind(&VescInterface::Impl::serial_receive_callback, this, std::placeholders::_1)); } - } VescInterface::VescInterface( @@ -222,12 +223,9 @@ void VescInterface::disconnect() bool VescInterface::isConnected() const { auto port = impl_->serial_driver_->port(); - if (port) - { + if (port) { return port->is_open(); - } - else - { + } else { return false; } } From e680b30564139ffabd41a6f731dbc41145e6f854 Mon Sep 17 00:00:00 2001 From: Jacob Johnson Date: Wed, 27 Oct 2021 19:27:36 -0700 Subject: [PATCH 49/72] change subscriber msg type to Float --- vesc_ackermann/src/vesc_to_odom.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 1deef6c..712cf12 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -42,6 +42,7 @@ namespace vesc_ackermann using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::Odometry; using std::placeholders::_1; +using std_msgs::msg::Float64; using vesc_msgs::msg::VescStateStamped; VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) @@ -83,7 +84,7 @@ VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) "sensors/core", 10, std::bind(&VescToOdom::vescStateCallback, this, _1)); if (use_servo_cmd_) { - servo_sub_ = create_subscription( + servo_sub_ = create_subscription( "sensors/servo_position_command", 10, std::bind(&VescToOdom::servoCmdCallback, this, _1)); } } From 8aadb176b31faa23e3df1289b0e5d9c6b5b59537 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Fri, 11 Jun 2021 20:41:01 +0200 Subject: [PATCH 50/72] added program to enumerate vesc serial ports based on their uuid --- vesc_driver/CMakeLists.txt | 6 ++ .../vesc_driver/vesc_device_uuid_lookup.hpp | 72 +++++++++++++++ vesc_driver/src/vesc_device_namer.cpp | 47 ++++++++++ vesc_driver/src/vesc_device_uuid_lookup.cpp | 88 +++++++++++++++++++ vesc_driver/src/vesc_interface.cpp | 2 +- 5 files changed, 214 insertions(+), 1 deletion(-) create mode 100644 vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp create mode 100644 vesc_driver/src/vesc_device_namer.cpp create mode 100644 vesc_driver/src/vesc_device_uuid_lookup.cpp diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index 1f0f4ba..d3dad25 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -34,6 +34,12 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_node ) +ament_auto_add_executable( + vesc_device_namer + src/vesc_device_namer.cpp + src/vesc_device_uuid_lookup.cpp +) + ############# ## Testing ## ############# diff --git a/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp b/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp new file mode 100644 index 0000000..418fcb9 --- /dev/null +++ b/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp @@ -0,0 +1,72 @@ +/* +# Copyright 2021 Andrea Scipone +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +*/ + +#ifndef VESC_DRIVER__VESC_DEVICE_UUID_LOOKUP_HPP_ +#define VESC_DRIVER__VESC_DEVICE_UUID_LOOKUP_HPP_ + + +#include +#include + +#include "vesc_driver/datatypes.hpp" +#include "vesc_driver/vesc_interface.hpp" +#include "vesc_driver/vesc_packet.hpp" + +namespace vesc_driver +{ + +class VescDeviceLookup +{ + public: + VescDeviceLookup(std::string device); + + const char * deviceUUID() const; + const char * version() const; + const char * hwname() const; + void close(); + bool isReady(); + private: + std::string device_; + std::string uuid_; + std::string version_; + std::string hwname_; + std::string error_; + bool ready_; + + private: + // interface to the VESC + VescInterface vesc_; + void vescPacketCallback(const std::shared_ptr& packet); + void vescErrorCallback(const std::string& error); + + static std::string decode_uuid(const uint8_t * uuid) + { + char uuid_data[100] = ""; + std::string uuid_founded; + snprintf( + uuid_data, sizeof(uuid_data), + "%02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x", + uuid[0], uuid[1], uuid[2], + uuid[3], uuid[4], uuid[5], + uuid[6], uuid[7], uuid[8], + uuid[9], uuid[10], uuid[11] + ); + return uuid_data; + } + }; +} // namespace vesc_driver + +#endif // VESC_DRIVER__VESC_DEVICE_UUID_LOOKUP_HPP_ diff --git a/vesc_driver/src/vesc_device_namer.cpp b/vesc_driver/src/vesc_device_namer.cpp new file mode 100644 index 0000000..ee980cf --- /dev/null +++ b/vesc_driver/src/vesc_device_namer.cpp @@ -0,0 +1,47 @@ +/* +# Copyright 2021 Andrea Scipone +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +*/ + +#include +#include + +#include +#include +#include + +int main(int argc, char** argv) +{ + std::string devicePort = (argc > 1? argv[1]:"ttyACM0"); + std::string maxRetry_ = (argc > 2? argv[2]:"50"); + std::string VESC_UUID_ENV = "VESC_UUID_ENV="; + + vesc_driver::VescDeviceLookup lookup("/dev/" + devicePort); + int maxRetry = stoi(maxRetry_); + for (int i = 0;i < maxRetry;i++) { + usleep(20); + if (lookup.isReady()) break; + } + + if(lookup.isReady()) + { + VESC_UUID_ENV += lookup.deviceUUID(); + + std::cout << lookup.deviceUUID() << std::endl; + setenv("VESC_UUID_ENV", lookup.deviceUUID(), true); + return 0; + } else { + return -1; + } +} diff --git a/vesc_driver/src/vesc_device_uuid_lookup.cpp b/vesc_driver/src/vesc_device_uuid_lookup.cpp new file mode 100644 index 0000000..6372927 --- /dev/null +++ b/vesc_driver/src/vesc_device_uuid_lookup.cpp @@ -0,0 +1,88 @@ +/* +# Copyright 2021 Andrea Scipone +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +*/ + +#include "vesc_driver/vesc_device_uuid_lookup.hpp" + +#include +#include +#include +#include +#include +#include + +namespace vesc_driver +{ + + +VescDeviceLookup::VescDeviceLookup(std::string name) +: + vesc_( + std::string(), + boost::bind(&VescDeviceLookup::vescPacketCallback, this, _1), + boost::bind(&VescDeviceLookup::vescErrorCallback, this, _1) + ), + ready_(false), + device_(name) + { + try { + vesc_.connect(device_); + vesc_.requestFWVersion(); + } + catch (SerialException e) { + std::cerr << "VESC error on port " << device_ << std::endl << e.what() << std::endl; + return; + } + } +void VescDeviceLookup::close(){ + vesc_.disconnect(); +} + +void VescDeviceLookup::vescPacketCallback(const std::shared_ptr & packet) { + if (packet->name() == "FWVersion") { + std::shared_ptr fw_version = + std::dynamic_pointer_cast(packet); + + const uint8_t *uuid= fw_version->uuid(); + + hwname_ = fw_version->hwname(); + version_ = fw_version->fwMajor() + "." + fw_version->fwMinor(); + uuid_ += decode_uuid(uuid); + ready_=true; + } +} + +void VescDeviceLookup::vescErrorCallback(const std::string & error) { + error_ = error; + ready_ = false; +} + +bool VescDeviceLookup::isReady(){ + return ready_; +} + +const char* VescDeviceLookup::deviceUUID() const { + return uuid_.c_str(); +} + + +const char * VescDeviceLookup::version() const { + return version_.c_str(); +} +const char* VescDeviceLookup::hwname() const { + return hwname_.c_str(); +} + +} // namespace vesc_driver diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 9ccf4d2..6f8f55d 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -130,7 +130,6 @@ void VescInterface::Impl::packet_creation_thread() // erase "used" buffer buffer_.erase(buffer_.begin(), iter); } - buffer_mutex_.unlock(); // Only attempt to read every 10 ms std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -215,6 +214,7 @@ void VescInterface::disconnect() if (isConnected()) { // bring down read thread impl_->packet_thread_run_ = false; + requestFWVersion(); impl_->packet_thread_->join(); impl_->serial_driver_->port()->close(); } From 7146d28216f894b591ad512489a58b116d42779d Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Mon, 21 Jun 2021 22:34:10 +0200 Subject: [PATCH 51/72] added script for udev --- vesc_driver/scripts/99-vesc6.rules | 9 +++++++++ vesc_driver/scripts/vesc_device_lookup | 5 +++++ 2 files changed, 14 insertions(+) create mode 100644 vesc_driver/scripts/99-vesc6.rules create mode 100755 vesc_driver/scripts/vesc_device_lookup diff --git a/vesc_driver/scripts/99-vesc6.rules b/vesc_driver/scripts/99-vesc6.rules new file mode 100644 index 0000000..839c7d2 --- /dev/null +++ b/vesc_driver/scripts/99-vesc6.rules @@ -0,0 +1,9 @@ +# Device vesc6 udev definition +# idVendor 0x0403 +# idProduct 0x5740 + + +# Alias the Sparkfun 9DoF as imu, and the electronic speed controller as VESC +#ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="1b4f", ATTRS{idProduct}=="9d0f", SYMLINK+="imu" , GROUP="dialout" +#ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", SYMLINK+="vesc%n", GROUP="dialout" +ACTION=="add", SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740",PROGRAM="/bin/vesc_device_lookup %k", SYMLINK+="vesc/%c", GROUP="dialout" \ No newline at end of file diff --git a/vesc_driver/scripts/vesc_device_lookup b/vesc_driver/scripts/vesc_device_lookup new file mode 100755 index 0000000..59da038 --- /dev/null +++ b/vesc_driver/scripts/vesc_device_lookup @@ -0,0 +1,5 @@ +dme#!/bin/bash + +export LD_LIBRARY_PATH=/opt/ros/foxy/lib + +vesc_device_namer $1 1000 From 59a27e2273818a54fb04d405f4e93f0026c852f6 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Tue, 22 Jun 2021 00:17:04 +0200 Subject: [PATCH 52/72] fixed test for udev --- .../vesc_driver/vesc_device_uuid_lookup.hpp | 71 ++++++++++--------- vesc_driver/src/vesc_device_namer.cpp | 40 +++++------ vesc_driver/src/vesc_device_uuid_lookup.cpp | 66 +++++++++-------- 3 files changed, 92 insertions(+), 85 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp b/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp index 418fcb9..673a69f 100644 --- a/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp +++ b/vesc_driver/include/vesc_driver/vesc_device_uuid_lookup.hpp @@ -30,43 +30,44 @@ namespace vesc_driver class VescDeviceLookup { - public: - VescDeviceLookup(std::string device); +public: + explicit VescDeviceLookup(std::string device); - const char * deviceUUID() const; - const char * version() const; - const char * hwname() const; - void close(); - bool isReady(); - private: - std::string device_; - std::string uuid_; - std::string version_; - std::string hwname_; - std::string error_; - bool ready_; + const char * deviceUUID() const; + const char * version() const; + const char * hwname() const; + void close(); + bool isReady(); - private: - // interface to the VESC - VescInterface vesc_; - void vescPacketCallback(const std::shared_ptr& packet); - void vescErrorCallback(const std::string& error); - - static std::string decode_uuid(const uint8_t * uuid) - { - char uuid_data[100] = ""; - std::string uuid_founded; - snprintf( - uuid_data, sizeof(uuid_data), - "%02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x", - uuid[0], uuid[1], uuid[2], - uuid[3], uuid[4], uuid[5], - uuid[6], uuid[7], uuid[8], - uuid[9], uuid[10], uuid[11] - ); - return uuid_data; - } - }; +private: + std::string device_; + std::string uuid_; + std::string version_; + std::string hwname_; + std::string error_; + bool ready_; + +private: + // interface to the VESC + VescInterface vesc_; + void vescPacketCallback(const std::shared_ptr & packet); + void vescErrorCallback(const std::string & error); + + static std::string decode_uuid(const uint8_t * uuid) + { + char uuid_data[100] = ""; + std::string uuid_founded; + snprintf( + uuid_data, sizeof(uuid_data), + "%02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x", + uuid[0], uuid[1], uuid[2], + uuid[3], uuid[4], uuid[5], + uuid[6], uuid[7], uuid[8], + uuid[9], uuid[10], uuid[11] + ); + return uuid_data; + } +}; } // namespace vesc_driver #endif // VESC_DRIVER__VESC_DEVICE_UUID_LOOKUP_HPP_ diff --git a/vesc_driver/src/vesc_device_namer.cpp b/vesc_driver/src/vesc_device_namer.cpp index ee980cf..9957731 100644 --- a/vesc_driver/src/vesc_device_namer.cpp +++ b/vesc_driver/src/vesc_device_namer.cpp @@ -16,32 +16,30 @@ #include #include - +#include #include #include -#include -int main(int argc, char** argv) +int main(int argc, char ** argv) { - std::string devicePort = (argc > 1? argv[1]:"ttyACM0"); - std::string maxRetry_ = (argc > 2? argv[2]:"50"); - std::string VESC_UUID_ENV = "VESC_UUID_ENV="; + std::string devicePort = (argc > 1 ? argv[1] : "ttyACM0"); + std::string maxRetry_ = (argc > 2 ? argv[2] : "50"); + std::string VESC_UUID_ENV = "VESC_UUID_ENV="; - vesc_driver::VescDeviceLookup lookup("/dev/" + devicePort); - int maxRetry = stoi(maxRetry_); - for (int i = 0;i < maxRetry;i++) { - usleep(20); - if (lookup.isReady()) break; - } + vesc_driver::VescDeviceLookup lookup("/dev/" + devicePort); + int maxRetry = stoi(maxRetry_); + for (int i = 0; i < maxRetry; i++) { + usleep(20); + if (lookup.isReady()) {break;} + } - if(lookup.isReady()) - { - VESC_UUID_ENV += lookup.deviceUUID(); + if (lookup.isReady()) { + VESC_UUID_ENV += lookup.deviceUUID(); - std::cout << lookup.deviceUUID() << std::endl; - setenv("VESC_UUID_ENV", lookup.deviceUUID(), true); - return 0; - } else { - return -1; - } + std::cout << lookup.deviceUUID() << std::endl; + setenv("VESC_UUID_ENV", lookup.deviceUUID(), true); + return 0; + } else { + return -1; + } } diff --git a/vesc_driver/src/vesc_device_uuid_lookup.cpp b/vesc_driver/src/vesc_device_uuid_lookup.cpp index 6372927..3ca64f6 100644 --- a/vesc_driver/src/vesc_device_uuid_lookup.cpp +++ b/vesc_driver/src/vesc_device_uuid_lookup.cpp @@ -13,75 +13,83 @@ # See the License for the specific language governing permissions and # limitations under the License. */ - -#include "vesc_driver/vesc_device_uuid_lookup.hpp" - #include #include #include #include #include +#include +#include #include +#include "vesc_driver/vesc_device_uuid_lookup.hpp" + namespace vesc_driver { VescDeviceLookup::VescDeviceLookup(std::string name) -: - vesc_( - std::string(), - boost::bind(&VescDeviceLookup::vescPacketCallback, this, _1), - boost::bind(&VescDeviceLookup::vescErrorCallback, this, _1) - ), +: vesc_( + std::string(), + boost::bind(&VescDeviceLookup::vescPacketCallback, this, _1), + boost::bind(&VescDeviceLookup::vescErrorCallback, this, _1) +), ready_(false), device_(name) - { - try { - vesc_.connect(device_); - vesc_.requestFWVersion(); - } - catch (SerialException e) { - std::cerr << "VESC error on port " << device_ << std::endl << e.what() << std::endl; - return; - } - } -void VescDeviceLookup::close(){ +{ + try { + vesc_.connect(device_); + vesc_.requestFWVersion(); + } catch (SerialException e) { + std::cerr << "VESC error on port " << device_ << std::endl << e.what() << std::endl; + return; + } +} + +void VescDeviceLookup::close() +{ vesc_.disconnect(); } -void VescDeviceLookup::vescPacketCallback(const std::shared_ptr & packet) { +void VescDeviceLookup::vescPacketCallback(const std::shared_ptr & packet) +{ if (packet->name() == "FWVersion") { std::shared_ptr fw_version = std::dynamic_pointer_cast(packet); - const uint8_t *uuid= fw_version->uuid(); + const uint8_t * uuid = fw_version->uuid(); hwname_ = fw_version->hwname(); version_ = fw_version->fwMajor() + "." + fw_version->fwMinor(); uuid_ += decode_uuid(uuid); - ready_=true; + ready_ = true; } } - -void VescDeviceLookup::vescErrorCallback(const std::string & error) { + +void VescDeviceLookup::vescErrorCallback(const std::string & error) +{ error_ = error; ready_ = false; } -bool VescDeviceLookup::isReady(){ +bool VescDeviceLookup::isReady() +{ return ready_; } -const char* VescDeviceLookup::deviceUUID() const { +const char * VescDeviceLookup::deviceUUID() const +{ return uuid_.c_str(); } -const char * VescDeviceLookup::version() const { +const char * VescDeviceLookup::version() const +{ return version_.c_str(); } -const char* VescDeviceLookup::hwname() const { + +const char * VescDeviceLookup::hwname() const +{ return hwname_.c_str(); } From 4a9aafd4a2494a789e691144a946f0cd538391b4 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Tue, 22 Jun 2021 00:18:42 +0200 Subject: [PATCH 53/72] added imu polling --- vesc_driver/CMakeLists.txt | 4 +- .../include/vesc_driver/vesc_driver.hpp | 4 + .../include/vesc_driver/vesc_interface.hpp | 2 + .../include/vesc_driver/vesc_packet.hpp | 62 +++++++ vesc_driver/package.xml | 2 + vesc_driver/src/vesc_driver.cpp | 47 +++++ vesc_driver/src/vesc_interface.cpp | 5 + vesc_driver/src/vesc_packet.cpp | 162 ++++++++++++++++++ vesc_msgs/CMakeLists.txt | 2 + vesc_msgs/msg/VescImu.msg | 8 + vesc_msgs/msg/VescImuStamped.msg | 4 + 11 files changed, 300 insertions(+), 2 deletions(-) create mode 100644 vesc_msgs/msg/VescImu.msg create mode 100644 vesc_msgs/msg/VescImuStamped.msg diff --git a/vesc_driver/CMakeLists.txt b/vesc_driver/CMakeLists.txt index d3dad25..5d3eac0 100644 --- a/vesc_driver/CMakeLists.txt +++ b/vesc_driver/CMakeLists.txt @@ -35,8 +35,8 @@ rclcpp_components_register_node(${PROJECT_NAME} ) ament_auto_add_executable( - vesc_device_namer - src/vesc_device_namer.cpp + vesc_device_namer + src/vesc_device_namer.cpp src/vesc_device_uuid_lookup.cpp ) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index 9377c36..4545448 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -35,6 +35,8 @@ #include #include #include +#include +#include #include #include @@ -49,6 +51,7 @@ namespace vesc_driver using std_msgs::msg::Float64; using vesc_msgs::msg::VescState; using vesc_msgs::msg::VescStateStamped; +using vesc_msgs::msg::VescImuStamped; class VescDriver : public rclcpp::Node @@ -88,6 +91,7 @@ class VescDriver // ROS services rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; rclcpp::Publisher::SharedPtr servo_sensor_pub_; rclcpp::SubscriptionBase::SharedPtr duty_cycle_sub_; rclcpp::SubscriptionBase::SharedPtr current_sub_; diff --git a/vesc_driver/include/vesc_driver/vesc_interface.hpp b/vesc_driver/include/vesc_driver/vesc_interface.hpp index 63724a1..dcca50f 100644 --- a/vesc_driver/include/vesc_driver/vesc_interface.hpp +++ b/vesc_driver/include/vesc_driver/vesc_interface.hpp @@ -116,6 +116,8 @@ class VescInterface void requestFWVersion(); void requestState(); + void requestImuData(); + void setDutyCycle(double duty_cycle); void setCurrent(double current); void setBrake(double brake); diff --git a/vesc_driver/include/vesc_driver/vesc_packet.hpp b/vesc_driver/include/vesc_driver/vesc_packet.hpp index 74ccd72..950ad91 100644 --- a/vesc_driver/include/vesc_driver/vesc_packet.hpp +++ b/vesc_driver/include/vesc_driver/vesc_packet.hpp @@ -240,6 +240,68 @@ class VescPacketSetServoPos : public VescPacket // double servo_pos() const; }; +/*------------------------------------------------------------------------------------------------*/ +class VescPacketRequestImu : public VescPacket +{ +public: + VescPacketRequestImu(); +}; + +class VescPacketImu : public VescPacket +{ +public: + explicit VescPacketImu(std::shared_ptr raw); + + int mask() const; + + double yaw() const; + double pitch() const; + double roll() const; + + double acc_x() const; + double acc_y() const; + double acc_z() const; + + double gyr_x() const; + double gyr_y() const; + double gyr_z() const; + + double mag_x() const; + double mag_y() const; + double mag_z() const; + + double q_w() const; + double q_x() const; + double q_y() const; + double q_z() const; + +private: + double getFloat32Auto(uint32_t * pos) const; + + uint32_t mask_; + double roll_; + double pitch_; + double yaw_; + + double acc_x_; + double acc_y_; + double acc_z_; + + double gyr_x_; + double gyr_y_; + double gyr_z_; + + double mag_x_; + double mag_y_; + double mag_z_; + + double q0_; + double q1_; + double q2_; + double q3_; +}; + +/*------------------------------------------------------------------------------------------------*/ } // namespace vesc_driver #endif // VESC_DRIVER__VESC_PACKET_HPP_ diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index ef527ed..4a44d77 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -22,6 +22,8 @@ std_msgs vesc_msgs serial_driver + sensors_msgs + ament_lint_auto ament_lint_common diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index 5db7395..dfa18a8 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -78,6 +78,7 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) // create vesc state (telemetry) publisher state_pub_ = create_publisher("sensors/core", rclcpp::QoS{10}); + imu_pub_ = create_publisher("sensors/imu", rclcpp::QoS{10}); // since vesc state does not include the servo position, publish the commanded // servo position as a "sensor" @@ -143,6 +144,8 @@ void VescDriver::timerCallback() } else if (driver_mode_ == MODE_OPERATING) { // poll for vesc state (telemetry) vesc_.requestState(); + // poll for vesc imu + vesc_.requestImuData(); } else { // unknown mode, how did that happen? assert(false && "unknown driver mode"); @@ -190,7 +193,51 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa // todo: might need lock here fw_version_major_ = fw_version->fwMajor(); fw_version_minor_ = fw_version->fwMinor(); + RCLCPP_INFO( + get_logger(), + "-=%s=- hardware paired %d", + fw_version->hwname().c_str(), + fw_version->paired() + ); + } else if (packet->name() == "ImuData") { + std::shared_ptr imuData = + std::dynamic_pointer_cast(packet); + + auto imu_msg = VescImuStamped(); + imu_msg.header.stamp = now(); + + imu_msg.imu.ypr.x = imuData->roll(); + imu_msg.imu.ypr.y = imuData->pitch(); + imu_msg.imu.ypr.z = imuData->yaw(); + + imu_msg.imu.linear_acceleration.x = imuData->acc_x(); + imu_msg.imu.linear_acceleration.y = imuData->acc_y(); + imu_msg.imu.linear_acceleration.z = imuData->acc_z(); + + imu_msg.imu.angular_velocity.x = imuData->gyr_x(); + imu_msg.imu.angular_velocity.y = imuData->gyr_y(); + imu_msg.imu.angular_velocity.z = imuData->gyr_z(); + + + imu_msg.imu.compass.x = imuData->mag_x(); + imu_msg.imu.compass.y = imuData->mag_y(); + imu_msg.imu.compass.z = imuData->mag_z(); + + imu_msg.imu.orientation.w = imuData->q_w(); + imu_msg.imu.orientation.x = imuData->q_x(); + imu_msg.imu.orientation.y = imuData->q_y(); + imu_msg.imu.orientation.z = imuData->q_z(); + + imu_pub_->publish(imu_msg); } + auto & clk = *this->get_clock(); + RCLCPP_INFO_THROTTLE( + get_logger(), + clk, + 5000, + "%s packet received", + packet->name().c_str() + ); } void VescDriver::vescErrorCallback(const std::string & error) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index 6f8f55d..dd753cf 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -275,4 +275,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 1cb9980..4975cbc 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -524,4 +524,166 @@ VescPacketSetServoPos::VescPacketSetServoPos(double servo_pos) *(frame_->end() - 2) = static_cast(crc & 0xFF); } + +VescPacketImu::VescPacketImu(std::shared_ptr raw) +: VescPacket("ImuData", raw) +{ + uint32_t ind = 1; + mask_ = static_cast( + (static_cast(*(payload_.first + ind )) << 8) + + static_cast(*(payload_.first + ind + 1 )) + ); + ind += 2; + + if (mask_ & ((uint32_t)1 << 0)) {roll_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 1)) {pitch_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 2)) {yaw_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 3)) {acc_x_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 4)) {acc_y_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 5)) {acc_z_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 6)) {gyr_x_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 7)) {gyr_y_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 8)) {gyr_z_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 9)) {mag_x_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 10)) {mag_y_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 11)) {mag_z_ = getFloat32Auto(&ind);} + + if (mask_ & ((uint32_t)1 << 12)) {q0_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 13)) {q1_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 14)) {q2_ = getFloat32Auto(&ind);} + if (mask_ & ((uint32_t)1 << 15)) {q3_ = getFloat32Auto(&ind);} +} + +int VescPacketImu::mask() const +{ + return mask_; +} + +double VescPacketImu::getFloat32Auto(uint32_t * idx) const +{ + int pos = *idx; + uint32_t res = static_cast( + (static_cast(*(payload_.first + (pos ) )) << 24) + + (static_cast(*(payload_.first + (pos + 1) )) << 16) + + (static_cast(*(payload_.first + (pos + 2) )) << 8) + + static_cast(*(payload_.first + (pos + 3) )) + ); + + *idx += 4; + + int e = (res >> 23) & 0xFF; + int fr = res & 0x7FFFFF; + bool negative = res & (1 << 31); + + float f = 0.0; + if (e != 0 || fr != 0) { + f = static_cast(fr) / (8388608.0 * 2.0) + 0.5; + e -= 126; + } + + if (negative) { + f = -f; + } + return ldexpf(f, e); +} + +double VescPacketImu::roll() const +{ + return roll_ * 180 / M_PI; // da rad a gradi per debug deg +} + +double VescPacketImu::pitch() const +{ + return pitch_ * 180 / M_PI; +} + + +double VescPacketImu::yaw() const +{ + return yaw_ * 180 / M_PI; +} + + +double VescPacketImu::acc_x()const +{ + return acc_x_; // g/s +} + +double VescPacketImu::acc_y() const +{ + return acc_y_; +} + +double VescPacketImu::acc_z() const +{ + return acc_z_; +} + +double VescPacketImu::gyr_x() const +{ + return gyr_x_; // deg/s +} + +double VescPacketImu::gyr_y() const +{ + return gyr_y_; +} + +double VescPacketImu::gyr_z() const +{ + return gyr_z_; +} + +double VescPacketImu::mag_x() const +{ + return mag_x_; +} + +double VescPacketImu::mag_y() const +{ + return mag_y_; +} + +double VescPacketImu::mag_z() const +{ + return mag_z_; +} + +double VescPacketImu::q_w() const +{ + return q0_; +} + +double VescPacketImu::q_x() const +{ + return q1_; +} + +double VescPacketImu::q_y() const +{ + return q2_; +} + +double VescPacketImu::q_z() const +{ + return q3_; +} + +REGISTER_PACKET_TYPE(COMM_GET_IMU_DATA, VescPacketImu) + +VescPacketRequestImu::VescPacketRequestImu() +: VescPacket("RequestImuData", 3, COMM_GET_IMU_DATA) +{ + *(payload_.first + 1) = static_cast(0xFF); + *(payload_.first + 2) = static_cast(0xFF); + + uint16_t crc = CRC::Calculate( + &(*payload_.first), std::distance(payload_.first, payload_.second), VescFrame::CRC_TYPE); + *(frame_->end() - 3) = static_cast(crc >> 8); + *(frame_->end() - 2) = static_cast(crc & 0xFF); +} +/*------------------------------------------------------------------------------------------------*/ } // namespace vesc_driver diff --git a/vesc_msgs/CMakeLists.txt b/vesc_msgs/CMakeLists.txt index 8b222b0..88716cc 100644 --- a/vesc_msgs/CMakeLists.txt +++ b/vesc_msgs/CMakeLists.txt @@ -7,6 +7,8 @@ ament_auto_find_build_dependencies() rosidl_generate_interfaces(vesc_msgs "msg/VescState.msg" "msg/VescStateStamped.msg" + "msg/VescImu.msg" + "msg/VescImuStamped.msg" DEPENDENCIES builtin_interfaces std_msgs diff --git a/vesc_msgs/msg/VescImu.msg b/vesc_msgs/msg/VescImu.msg new file mode 100644 index 0000000..2765024 --- /dev/null +++ b/vesc_msgs/msg/VescImu.msg @@ -0,0 +1,8 @@ + + +geometry_msgs/Vector3 ypr +geometry_msgs/Vector3 linear_acceleration +geometry_msgs/Vector3 angular_velocity + +geometry_msgs/Vector3 compass +geometry_msgs/Quaternion orientation diff --git a/vesc_msgs/msg/VescImuStamped.msg b/vesc_msgs/msg/VescImuStamped.msg new file mode 100644 index 0000000..c8915b1 --- /dev/null +++ b/vesc_msgs/msg/VescImuStamped.msg @@ -0,0 +1,4 @@ + + +std_msgs/Header header +VescImu imu \ No newline at end of file From e500399255761ae0565fea5398ed8f01752245fd Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Tue, 22 Jun 2021 18:26:48 +0200 Subject: [PATCH 54/72] fixed timing wait for imu polling --- vesc_driver/package.xml | 1 + vesc_driver/src/vesc_interface.cpp | 2 +- vesc_msgs/CMakeLists.txt | 1 + vesc_msgs/package.xml | 1 + 4 files changed, 4 insertions(+), 1 deletion(-) diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index 4a44d77..f715e56 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -20,6 +20,7 @@ rclcpp rclcpp_components std_msgs + geometry_msgs vesc_msgs serial_driver sensors_msgs diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index dd753cf..c887925 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -132,7 +132,7 @@ void VescInterface::Impl::packet_creation_thread() } buffer_mutex_.unlock(); // Only attempt to read every 10 ms - std::this_thread::sleep_for(std::chrono::milliseconds(10)); +// std::this_thread::sleep_for(std::chrono::milliseconds(5)); } } diff --git a/vesc_msgs/CMakeLists.txt b/vesc_msgs/CMakeLists.txt index 88716cc..14ee8a2 100644 --- a/vesc_msgs/CMakeLists.txt +++ b/vesc_msgs/CMakeLists.txt @@ -12,6 +12,7 @@ rosidl_generate_interfaces(vesc_msgs DEPENDENCIES builtin_interfaces std_msgs + geometry_msgs ) if(BUILD_TESTING) diff --git a/vesc_msgs/package.xml b/vesc_msgs/package.xml index db7960a..60cc2ba 100644 --- a/vesc_msgs/package.xml +++ b/vesc_msgs/package.xml @@ -19,6 +19,7 @@ rosidl_default_generators builtin_interfaces std_msgs + geometry_msgs rosidl_default_runtime ament_lint_auto From a8dbe88e3e0c81a0f620e03f8257cfa964b94368 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Wed, 23 Jun 2021 08:21:21 +0200 Subject: [PATCH 55/72] fix sensor --- vesc_driver/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/package.xml b/vesc_driver/package.xml index f715e56..c40e286 100644 --- a/vesc_driver/package.xml +++ b/vesc_driver/package.xml @@ -23,7 +23,7 @@ geometry_msgs vesc_msgs serial_driver - sensors_msgs + sensor_msgs ament_lint_auto From 68c16643bb773c299aa4755193adb5c408f05fd5 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Sat, 3 Jul 2021 14:38:17 +0200 Subject: [PATCH 56/72] replaced boost::bind with std::bind --- vesc_driver/src/vesc_device_uuid_lookup.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/vesc_driver/src/vesc_device_uuid_lookup.cpp b/vesc_driver/src/vesc_device_uuid_lookup.cpp index 3ca64f6..11d3e45 100644 --- a/vesc_driver/src/vesc_device_uuid_lookup.cpp +++ b/vesc_driver/src/vesc_device_uuid_lookup.cpp @@ -20,19 +20,19 @@ #include #include #include -#include + #include "vesc_driver/vesc_device_uuid_lookup.hpp" namespace vesc_driver { - +using std::placeholders::_1; VescDeviceLookup::VescDeviceLookup(std::string name) : vesc_( std::string(), - boost::bind(&VescDeviceLookup::vescPacketCallback, this, _1), - boost::bind(&VescDeviceLookup::vescErrorCallback, this, _1) + std::bind(&VescDeviceLookup::vescPacketCallback, this, _1), + std::bind(&VescDeviceLookup::vescErrorCallback, this, _1) ), ready_(false), device_(name) From 6b1838694b06484a7f4cdd21af602a1851032804 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Thu, 16 Sep 2021 11:00:16 +0200 Subject: [PATCH 57/72] publish sensors_msgs::Imu --- .../include/vesc_driver/vesc_driver.hpp | 5 ++++- vesc_driver/src/vesc_driver.cpp | 20 ++++++++++++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index 4545448..ba59fc1 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -32,12 +32,12 @@ #define VESC_DRIVER__VESC_DRIVER_HPP_ #include +#include #include #include #include #include #include - #include #include #include @@ -52,6 +52,7 @@ using std_msgs::msg::Float64; using vesc_msgs::msg::VescState; using vesc_msgs::msg::VescStateStamped; using vesc_msgs::msg::VescImuStamped; +using sensor_msgs::msg::Imu; class VescDriver : public rclcpp::Node @@ -92,6 +93,8 @@ class VescDriver // ROS services rclcpp::Publisher::SharedPtr state_pub_; rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr imu_std_pub_; + rclcpp::Publisher::SharedPtr servo_sensor_pub_; rclcpp::SubscriptionBase::SharedPtr duty_cycle_sub_; rclcpp::SubscriptionBase::SharedPtr current_sub_; diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index dfa18a8..e0a7700 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -47,6 +47,7 @@ using namespace std::chrono_literals; using std::placeholders::_1; using std_msgs::msg::Float64; using vesc_msgs::msg::VescStateStamped; +using sensor_msgs::msg::Imu; VescDriver::VescDriver(const rclcpp::NodeOptions & options) : rclcpp::Node("vesc_driver", options), @@ -79,6 +80,7 @@ VescDriver::VescDriver(const rclcpp::NodeOptions & options) // create vesc state (telemetry) publisher state_pub_ = create_publisher("sensors/core", rclcpp::QoS{10}); imu_pub_ = create_publisher("sensors/imu", rclcpp::QoS{10}); + imu_std_pub_ = create_publisher("sensors/imu/raw", rclcpp::QoS{10}); // since vesc state does not include the servo position, publish the commanded // servo position as a "sensor" @@ -204,7 +206,9 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa std::dynamic_pointer_cast(packet); auto imu_msg = VescImuStamped(); + auto std_imu_msg = Imu(); imu_msg.header.stamp = now(); + std_imu_msg.header.stamp = now(); imu_msg.imu.ypr.x = imuData->roll(); imu_msg.imu.ypr.y = imuData->pitch(); @@ -218,7 +222,6 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa 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(); @@ -228,7 +231,22 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa imu_msg.imu.orientation.y = imuData->q_y(); imu_msg.imu.orientation.z = imuData->q_z(); + std_imu_msg.linear_acceleration.x = imuData->acc_x(); + std_imu_msg.linear_acceleration.y = imuData->acc_y(); + std_imu_msg.linear_acceleration.z = imuData->acc_z(); + + std_imu_msg.angular_velocity.x = imuData->gyr_x(); + std_imu_msg.angular_velocity.y = imuData->gyr_y(); + std_imu_msg.angular_velocity.z = imuData->gyr_z(); + + std_imu_msg.orientation.w = imuData->q_w(); + std_imu_msg.orientation.x = imuData->q_x(); + std_imu_msg.orientation.y = imuData->q_y(); + std_imu_msg.orientation.z = imuData->q_z(); + + imu_pub_->publish(imu_msg); + imu_std_pub_->publish(std_imu_msg); } auto & clk = *this->get_clock(); RCLCPP_INFO_THROTTLE( From ea93496262015bbc502d753d0894728468880862 Mon Sep 17 00:00:00 2001 From: Andrea Scipione Date: Fri, 12 Nov 2021 21:53:44 +0100 Subject: [PATCH 58/72] fixed some test issues --- vesc_driver/launch/vesc_driver_node.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/vesc_driver/launch/vesc_driver_node.launch.py b/vesc_driver/launch/vesc_driver_node.launch.py index 005d68b..e0e5c4d 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.py +++ b/vesc_driver/launch/vesc_driver_node.launch.py @@ -26,10 +26,11 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -from launch import LaunchDescription +import os + from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription from launch_ros.actions import Node -import os def generate_launch_description(): From 29c503831d0a253af236095fe4fcad07f77464b7 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Wed, 12 Jan 2022 23:48:21 -0800 Subject: [PATCH 59/72] use sync receive; add back error handler --- vesc_driver/src/vesc_interface.cpp | 40 +++++++++++++++++------------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/vesc_driver/src/vesc_interface.cpp b/vesc_driver/src/vesc_interface.cpp index c887925..9557431 100644 --- a/vesc_driver/src/vesc_interface.cpp +++ b/vesc_driver/src/vesc_interface.cpp @@ -54,8 +54,6 @@ class VescInterface::Impl : owned_ctx{new IoContext(2)}, serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx)} {} - - void serial_receive_callback(const std::vector & buffer); void packet_creation_thread(); void on_configure(); void connect(const std::string & port); @@ -65,7 +63,6 @@ class VescInterface::Impl PacketHandlerFunction packet_handler_; ErrorHandlerFunction error_handler_; std::unique_ptr device_config_; - std::mutex buffer_mutex_; std::string device_name_; std::unique_ptr owned_ctx{}; std::unique_ptr serial_driver_; @@ -81,22 +78,18 @@ class VescInterface::Impl std::vector buffer_; }; -void VescInterface::Impl::serial_receive_callback(const std::vector & buffer) -{ - buffer_mutex_.lock(); // wait untill the current buffer read finishes - buffer_.reserve(buffer_.size() + buffer.size()); - buffer_.insert(buffer_.end(), buffer.begin(), buffer.end()); - buffer_mutex_.unlock(); -} - void VescInterface::Impl::packet_creation_thread() { + static auto temp_buffer = Buffer(2048, 0); while (packet_thread_run_) { - buffer_mutex_.lock(); + const auto bytes_read = serial_driver_->port()->receive(temp_buffer); + buffer_.reserve(buffer_.size() + temp_buffer.size()); + buffer_.insert(buffer_.end(), temp_buffer.begin(), temp_buffer.begin() + bytes_read); int bytes_needed = VescFrame::VESC_MIN_FRAME_SIZE; if (!buffer_.empty()) { // search buffer for valid packet(s) auto iter = buffer_.begin(); + auto iter_begin = buffer_.begin(); while (iter != buffer_.end()) { // check if valid start-of-frame character if (VescFrame::VESC_SOF_VAL_SMALL_FRAME == *iter || @@ -107,15 +100,26 @@ void VescInterface::Impl::packet_creation_thread() VescPacketConstPtr packet = VescPacketFactory::createPacket(iter, buffer_.end(), &bytes_needed, &error); if (packet) { + // good packet, check if we skipped any data + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, unknown data leading valid frame. Discarding " << + std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); + } // call packet handler packet_handler_(packet); // update state iter = iter + packet->frame().size(); + iter_begin = iter; // continue to look for another frame in buffer continue; } else if (bytes_needed > 0) { // need more data, break out of while loop break; // for (iter_sof... + } else { + // else, this was not a packet, move on to next byte + error_handler_(error); } } @@ -128,11 +132,15 @@ void VescInterface::Impl::packet_creation_thread() } // erase "used" buffer + if (std::distance(iter_begin, iter) > 0) { + std::ostringstream ss; + ss << "Out-of-sync with VESC, discarding " << std::distance(iter_begin, iter) << " bytes."; + error_handler_(ss.str()); + } buffer_.erase(buffer_.begin(), iter); } - buffer_mutex_.unlock(); - // Only attempt to read every 10 ms -// std::this_thread::sleep_for(std::chrono::milliseconds(5)); + // Only attempt to read every 5 ms + std::this_thread::sleep_for(std::chrono::milliseconds(5)); } } @@ -147,8 +155,6 @@ void VescInterface::Impl::connect(const std::string & port) serial_driver_->init_port(port, *device_config_); if (!serial_driver_->port()->is_open()) { serial_driver_->port()->open(); - serial_driver_->port()->async_receive( - std::bind(&VescInterface::Impl::serial_receive_callback, this, std::placeholders::_1)); } } From c97d86f5310bea06b44053b39a4adc46fc5465db Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Wed, 12 Jan 2022 23:48:41 -0800 Subject: [PATCH 60/72] reduce print severity to debug --- vesc_driver/src/vesc_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index e0a7700..c2c0edd 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -249,7 +249,7 @@ void VescDriver::vescPacketCallback(const std::shared_ptr & pa imu_std_pub_->publish(std_imu_msg); } auto & clk = *this->get_clock(); - RCLCPP_INFO_THROTTLE( + RCLCPP_DEBUG_THROTTLE( get_logger(), clk, 5000, From 43f67d45e8dd174597cc39554c49c67e3d9f99b0 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Wed, 12 Jan 2022 23:53:25 -0800 Subject: [PATCH 61/72] update readme --- README.md | 26 +++----------------------- 1 file changed, 3 insertions(+), 23 deletions(-) diff --git a/README.md b/README.md index e96dbd2..09f8afc 100644 --- a/README.md +++ b/README.md @@ -4,28 +4,7 @@ Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details -# Development Log -Haoru Xue 8/6/2021 - -Completed: - -- Fall back to C++ 14 -- Enabled error handler - - -Haoru Xue 5/16/2021 - -Completed: - -- Ported the VESC package to ROS2 -- Sensor reading tested -- Speed and steerign commands tested - -Potential Improvements: - -- C++ 17 is currently required to replace `boost::optional` with `std::optional` -- Speed sensor reading is inverted -- Not made LifeCycleNode yet +This is a ROS2 implementation of the ROS1 driver using the new serial driver located in [transport drivers](https://github.com/ros-drivers/transport_drivers). ## How to test @@ -33,5 +12,6 @@ Potential Improvements: 2. `rosdep update && rosdep install --from-paths src -i -y` 3. Plug in the VESC with a USB cable. 4. Modify `vesc/vesc_driver/params/vesc_config.yaml` to reflect any changes. -5. Build the package `colcon build` +5. Build the packages `colcon build` 6. `ros2 launch vesc_driver vesc_driver_node.launch.py` +7. If prompted "permission denied" on the serial port: `sudo chmod 777 /dev/ttyACM0` From 9adb37e7f7ce3b1b81c33e807a7377e4dc6e8b27 Mon Sep 17 00:00:00 2001 From: Haoru Xue Date: Tue, 15 Feb 2022 21:37:49 -0800 Subject: [PATCH 62/72] debug vesc_to_odom tf not publishing --- vesc_ackermann/src/vesc_to_odom.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 712cf12..86dc28d 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -56,9 +56,9 @@ VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) yaw_(0.0) { // get ROS parameters - declare_parameter("odom_frame", odom_frame_); - declare_parameter("base_frame", base_frame_); - declare_parameter("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_); + odom_frame_ = declare_parameter("odom_frame", odom_frame_); + base_frame_ = declare_parameter("base_frame", base_frame_); + use_servo_cmd_ = declare_parameter("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_); speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain").get(); speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset").get(); @@ -69,7 +69,7 @@ VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) wheelbase_ = declare_parameter("wheelbase").get(); } - declare_parameter("publish_tf", publish_tf_); + publish_tf_ = declare_parameter("publish_tf", publish_tf_); // create odom publisher odom_pub_ = create_publisher("odom", 10); From 0242cd8901179dbbc55d1ace291f4affead62872 Mon Sep 17 00:00:00 2001 From: Alessandro Celeste Date: Sat, 4 Mar 2023 19:45:52 +0100 Subject: [PATCH 63/72] Fix issue with if statements. --- vesc_driver/src/vesc_driver.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/vesc_driver/src/vesc_driver.cpp b/vesc_driver/src/vesc_driver.cpp index c2c0edd..5c8ef62 100644 --- a/vesc_driver/src/vesc_driver.cpp +++ b/vesc_driver/src/vesc_driver.cpp @@ -270,7 +270,7 @@ void VescDriver::vescErrorCallback(const std::string & error) */ void VescDriver::dutyCycleCallback(const Float64::SharedPtr duty_cycle) { - if (driver_mode_ = MODE_OPERATING) { + if (driver_mode_ == MODE_OPERATING) { vesc_.setDutyCycle(duty_cycle_limit_.clip(duty_cycle->data)); } } @@ -282,7 +282,7 @@ void VescDriver::dutyCycleCallback(const Float64::SharedPtr duty_cycle) */ void VescDriver::currentCallback(const Float64::SharedPtr current) { - if (driver_mode_ = MODE_OPERATING) { + if (driver_mode_ == MODE_OPERATING) { vesc_.setCurrent(current_limit_.clip(current->data)); } } @@ -294,7 +294,7 @@ void VescDriver::currentCallback(const Float64::SharedPtr current) */ void VescDriver::brakeCallback(const Float64::SharedPtr brake) { - if (driver_mode_ = MODE_OPERATING) { + if (driver_mode_ == MODE_OPERATING) { vesc_.setBrake(brake_limit_.clip(brake->data)); } } @@ -307,7 +307,7 @@ void VescDriver::brakeCallback(const Float64::SharedPtr brake) */ void VescDriver::speedCallback(const Float64::SharedPtr speed) { - if (driver_mode_ = MODE_OPERATING) { + if (driver_mode_ == MODE_OPERATING) { vesc_.setSpeed(speed_limit_.clip(speed->data)); } } @@ -318,7 +318,7 @@ void VescDriver::speedCallback(const Float64::SharedPtr speed) */ void VescDriver::positionCallback(const Float64::SharedPtr position) { - if (driver_mode_ = MODE_OPERATING) { + if (driver_mode_ == MODE_OPERATING) { // ROS uses radians but VESC seems to use degrees. Convert to degrees. double position_deg = position_limit_.clip(position->data) * 180.0 / M_PI; vesc_.setPosition(position_deg); @@ -330,7 +330,7 @@ void VescDriver::positionCallback(const Float64::SharedPtr position) */ void VescDriver::servoCallback(const Float64::SharedPtr servo) { - if (driver_mode_ = MODE_OPERATING) { + if (driver_mode_ == MODE_OPERATING) { double servo_clipped(servo_limit_.clip(servo->data)); vesc_.setServo(servo_clipped); // publish clipped servo value as a "sensor" From 10966484454a718d1cf63808d10e65d0b41fa7e1 Mon Sep 17 00:00:00 2001 From: Alessandro Celeste Date: Sat, 4 Mar 2023 18:17:57 +0100 Subject: [PATCH 64/72] Expose VESC config from VESC driver launch file. --- vesc_driver/launch/vesc_driver_node.launch.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/vesc_driver/launch/vesc_driver_node.launch.py b/vesc_driver/launch/vesc_driver_node.launch.py index e0e5c4d..b9777e7 100644 --- a/vesc_driver/launch/vesc_driver_node.launch.py +++ b/vesc_driver/launch/vesc_driver_node.launch.py @@ -30,6 +30,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -41,11 +43,16 @@ def generate_launch_description(): 'vesc_config.yaml' ) return LaunchDescription([ + DeclareLaunchArgument( + name="config", + default_value=vesc_config, + description="VESC yaml configuration file.", + ), Node( package='vesc_driver', executable='vesc_driver_node', name='vesc_driver_node', - parameters=[vesc_config] + parameters=[LaunchConfiguration("config")] ), ]) From 0b6248e39ed2b7188ead5c2d140a1c0460f96c4a Mon Sep 17 00:00:00 2001 From: Valerio Magnago Date: Fri, 10 Mar 2023 18:37:09 +0100 Subject: [PATCH 65/72] Add ci for ros 2 Humble. --- .github/workflows/ros2-ci.yaml | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ros2-ci.yaml b/.github/workflows/ros2-ci.yaml index 2bb3487..b07e5aa 100644 --- a/.github/workflows/ros2-ci.yaml +++ b/.github/workflows/ros2-ci.yaml @@ -11,9 +11,19 @@ on: jobs: build-ros2: + strategy: + fail-fast: false + matrix: + config: + - { + ros_distro: "humble" + } + - { + ros_distro: "foxy" + } runs-on: ubuntu-latest container: - image: ros:foxy + image: ros:${{ matrix.config.ros_distro }} steps: - name: Checkout Repo uses: actions/checkout@v2 From 307f99d6dbe9313bda7b37023b8b078952cf6568 Mon Sep 17 00:00:00 2001 From: Valerio Magnago Date: Sun, 19 Mar 2023 02:01:01 +0100 Subject: [PATCH 66/72] Remove foxy workflow. --- .github/workflows/ros2-ci.yaml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.github/workflows/ros2-ci.yaml b/.github/workflows/ros2-ci.yaml index b07e5aa..4e27f67 100644 --- a/.github/workflows/ros2-ci.yaml +++ b/.github/workflows/ros2-ci.yaml @@ -18,9 +18,6 @@ jobs: - { ros_distro: "humble" } - - { - ros_distro: "foxy" - } runs-on: ubuntu-latest container: image: ros:${{ matrix.config.ros_distro }} From 4af6bd70fa265838595805e338d4bfcf08d2c476 Mon Sep 17 00:00:00 2001 From: Valerio Magnago Date: Sun, 19 Mar 2023 02:03:28 +0100 Subject: [PATCH 67/72] Make cpplint happy of includes order. --- vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp | 9 +++++---- vesc_ackermann/src/ackermann_to_vesc.cpp | 6 +++--- vesc_ackermann/src/vesc_to_odom.cpp | 6 +++--- vesc_driver/include/vesc_driver/vesc_driver.hpp | 7 ++++--- vesc_driver/src/vesc_device_namer.cpp | 6 ++++-- 5 files changed, 19 insertions(+), 15 deletions(-) diff --git a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp index 916f2ef..60eb3a7 100644 --- a/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp +++ b/vesc_ackermann/include/vesc_ackermann/vesc_to_odom.hpp @@ -31,15 +31,16 @@ #ifndef VESC_ACKERMANN__VESC_TO_ODOM_HPP_ #define VESC_ACKERMANN__VESC_TO_ODOM_HPP_ -#include -#include -#include #include -#include #include #include +#include +#include +#include +#include + namespace vesc_ackermann { diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index e26c4f1..771f078 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -30,13 +30,13 @@ #include "vesc_ackermann/ackermann_to_vesc.hpp" -#include -#include - #include #include #include +#include +#include + namespace vesc_ackermann { diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 86dc28d..68134b6 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -30,12 +30,12 @@ #include "vesc_ackermann/vesc_to_odom.hpp" -#include -#include - #include #include +#include +#include + namespace vesc_ackermann { diff --git a/vesc_driver/include/vesc_driver/vesc_driver.hpp b/vesc_driver/include/vesc_driver/vesc_driver.hpp index ba59fc1..eddf44d 100644 --- a/vesc_driver/include/vesc_driver/vesc_driver.hpp +++ b/vesc_driver/include/vesc_driver/vesc_driver.hpp @@ -31,6 +31,10 @@ #ifndef VESC_DRIVER__VESC_DRIVER_HPP_ #define VESC_DRIVER__VESC_DRIVER_HPP_ +#include +#include +#include + #include #include #include @@ -38,9 +42,6 @@ #include #include #include -#include -#include -#include #include "vesc_driver/vesc_interface.hpp" #include "vesc_driver/vesc_packet.hpp" diff --git a/vesc_driver/src/vesc_device_namer.cpp b/vesc_driver/src/vesc_device_namer.cpp index 9957731..b50c846 100644 --- a/vesc_driver/src/vesc_device_namer.cpp +++ b/vesc_driver/src/vesc_device_namer.cpp @@ -16,9 +16,11 @@ #include #include -#include -#include + #include +#include + +#include int main(int argc, char ** argv) { From b3c67abd493ddeb8d0ad3d2288d99e56e2b3df7d Mon Sep 17 00:00:00 2001 From: Valerio Magnago Date: Sun, 19 Mar 2023 02:04:22 +0100 Subject: [PATCH 68/72] Make cppcheck happy fixing overflow. --- vesc_driver/src/vesc_packet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vesc_driver/src/vesc_packet.cpp b/vesc_driver/src/vesc_packet.cpp index 4975cbc..b9ab26f 100644 --- a/vesc_driver/src/vesc_packet.cpp +++ b/vesc_driver/src/vesc_packet.cpp @@ -576,7 +576,7 @@ double VescPacketImu::getFloat32Auto(uint32_t * idx) const int e = (res >> 23) & 0xFF; int fr = res & 0x7FFFFF; - bool negative = res & (1 << 31); + bool negative = res & (1u << 31); float f = 0.0; if (e != 0 || fr != 0) { From ce260f89826dfa5e762db8acc750a910c8aef822 Mon Sep 17 00:00:00 2001 From: Valerio Magnago Date: Sun, 19 Mar 2023 02:05:00 +0100 Subject: [PATCH 69/72] Fix compilation on ros humble. --- vesc_ackermann/src/ackermann_to_vesc.cpp | 10 ++++++---- vesc_ackermann/src/vesc_to_odom.cpp | 12 +++++++----- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 771f078..79863f8 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -48,10 +48,12 @@ AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) : Node("ackermann_to_vesc_node", options) { // get conversion parameters - speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain").get(); - speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset").get(); - steering_to_servo_gain_ = declare_parameter("steering_angle_to_servo_gain").get(); - steering_to_servo_offset_ = declare_parameter("steering_angle_to_servo_offset").get(); + speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain"); + speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset"); + steering_to_servo_gain_ = + declare_parameter("steering_angle_to_servo_gain"); + steering_to_servo_offset_ = + declare_parameter("steering_angle_to_servo_offset"); // create publishers to vesc electric-RPM (speed) and servo commands erpm_pub_ = create_publisher("commands/motor/speed", 10); diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 68134b6..83a11fb 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -60,13 +60,15 @@ VescToOdom::VescToOdom(const rclcpp::NodeOptions & options) base_frame_ = declare_parameter("base_frame", base_frame_); use_servo_cmd_ = declare_parameter("use_servo_cmd_to_calc_angular_velocity", use_servo_cmd_); - speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain").get(); - speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset").get(); + speed_to_erpm_gain_ = declare_parameter("speed_to_erpm_gain"); + speed_to_erpm_offset_ = declare_parameter("speed_to_erpm_offset"); if (use_servo_cmd_) { - steering_to_servo_gain_ = declare_parameter("steering_angle_to_servo_gain").get(); - steering_to_servo_offset_ = declare_parameter("steering_angle_to_servo_offset").get(); - wheelbase_ = declare_parameter("wheelbase").get(); + steering_to_servo_gain_ = + declare_parameter("steering_angle_to_servo_gain"); + steering_to_servo_offset_ = + declare_parameter("steering_angle_to_servo_offset"); + wheelbase_ = declare_parameter("wheelbase"); } publish_tf_ = declare_parameter("publish_tf", publish_tf_); From e4f3e874335c5c2512045c659029c46675d14078 Mon Sep 17 00:00:00 2001 From: Adam Morrissett <22103567+adamlm@users.noreply.github.com> Date: Fri, 22 Mar 2024 15:55:17 -0400 Subject: [PATCH 70/72] Negate current speed variable for vesc_to_odom package (#1) This change is used for the C1T truck --- README.md | 5 +++++ vesc_ackermann/src/vesc_to_odom.cpp | 7 ++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 09f8afc..23410ca 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,11 @@ Packages to interface with Veddar VESC motor controllers. See https://vesc-proje This is a ROS2 implementation of the ROS1 driver using the new serial driver located in [transport drivers](https://github.com/ros-drivers/transport_drivers). +## Changes made from upstream + +* Updated `README.md` with this change notice +* Negated the `current_speed` value in `vesc_to_odom.cpp` + ## How to test 1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src`. diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 83a11fb..c136970 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -28,6 +28,9 @@ // -*- mode:c++; fill-column: 100; -*- +// Changes made from upstream +// - negated current_speed value + #include "vesc_ackermann/vesc_to_odom.hpp" #include @@ -98,8 +101,10 @@ void VescToOdom::vescStateCallback(const VescStateStamped::SharedPtr state) return; } + // Begin change from upstream // convert to engineering units - double current_speed = (-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; + double current_speed = -(-state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; + // End change from upstream if (std::fabs(current_speed) < 0.05) { current_speed = 0.0; } From f642c78045a251bb11712c3f9d6757005c4f1441 Mon Sep 17 00:00:00 2001 From: Saikrishna Bairamoni <84093461+SaikrishnaBairamoni@users.noreply.github.com> Date: Tue, 24 Sep 2024 14:19:45 -0400 Subject: [PATCH 71/72] Update README.md (#3) --- README.md | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 23410ca..c1b6b48 100644 --- a/README.md +++ b/README.md @@ -1,16 +1,11 @@ # Veddar VESC Interface -![ROS2 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS2%20CI%20Workflow/badge.svg) +Vesc is a new repository that was forked to provide a local instance for developing minor changes required for the C1T system. The only update included in this release is negating the speed value sent to the vesc due to the motors being installed in the opposite direction. Packages to interface with Veddar VESC motor controllers. See https://vesc-project.com/ for details This is a ROS2 implementation of the ROS1 driver using the new serial driver located in [transport drivers](https://github.com/ros-drivers/transport_drivers). -## Changes made from upstream - -* Updated `README.md` with this change notice -* Negated the `current_speed` value in `vesc_to_odom.cpp` - ## How to test 1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src`. @@ -20,3 +15,20 @@ This is a ROS2 implementation of the ROS1 driver using the new serial driver loc 5. Build the packages `colcon build` 6. `ros2 launch vesc_driver vesc_driver_node.launch.py` 7. If prompted "permission denied" on the serial port: `sudo chmod 777 /dev/ttyACM0` + +## Contribution +Welcome to the CARMA contributing guide. Please read this guide to learn about our development process, how to propose pull requests and improvements, and how to build and test your changes to this project. [CARMA Contributing Guide](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) + +## Code of Conduct +Please read our [CARMA Code of Conduct](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Code_of_Conduct.md) which outlines our expectations for participants within the CARMA community, as well as steps to reporting unacceptable behavior. We are committed to providing a welcoming and inspiring community for all and expect our code of conduct to be honored. Anyone who violates this code of conduct may be banned from the community. + +## Attribution +The development team would like to acknowledge the people who have made direct contributions to the design and code in this repository. [CARMA Attribution](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/ATTRIBUTION.txt) + +## License +By contributing to the Federal Highway Administration (FHWA) Connected Automated Research Mobility Applications (CARMA), you agree that your contributions will be licensed under its Apache License 2.0 license. [CARMA License](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/docs/License.md) + +## Contact +Please click on the CARMA logo below to visit the Federal Highway Adminstration(FHWA) CARMA website. + +[![CARMA Image](https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/docs/image/CARMA_icon.png)](https://highways.dot.gov/research/research-programs/operations/CARMA) From e3d647f23a88a022d19934ddc0bbfad121f80357 Mon Sep 17 00:00:00 2001 From: Tanuj-D Date: Wed, 16 Oct 2024 16:55:00 -0400 Subject: [PATCH 72/72] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c1b6b48..9cd4091 100644 --- a/README.md +++ b/README.md @@ -29,6 +29,6 @@ The development team would like to acknowledge the people who have made direct c By contributing to the Federal Highway Administration (FHWA) Connected Automated Research Mobility Applications (CARMA), you agree that your contributions will be licensed under its Apache License 2.0 license. [CARMA License](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/docs/License.md) ## Contact -Please click on the CARMA logo below to visit the Federal Highway Adminstration(FHWA) CARMA website. +Please click on the CARMA logo below to visit the Federal Highway Adminstration(FHWA) CARMA website. For technical support from the CARMA team, please contact the CARMA help desk at CAVSupportServices@dot.gov. [![CARMA Image](https://raw.githubusercontent.com/usdot-fhwa-stol/carma-platform/develop/docs/image/CARMA_icon.png)](https://highways.dot.gov/research/research-programs/operations/CARMA)