From 21ed1d0e3b3af3580aa0040791e83060eb3dfab0 Mon Sep 17 00:00:00 2001 From: Dexter Date: Mon, 29 Jan 2024 16:40:22 -0500 Subject: [PATCH] Added base template for sbus interface --- interfaces/kr_sbus_interface/CMakeLists.txt | 51 ++ .../kr_sbus_interface/cmake/FindSnav.cmake | 21 + .../kr_sbus_interface/config/gains.yaml | 15 + .../config/mav_manager_params.yaml | 6 + .../config/tracker_params.yaml | 25 + .../kr_sbus_interface/config/trackers.yaml | 11 + .../kr_sbus_interface/channel_mapping.h | 17 + .../include/kr_sbus_interface/sbus_bridge.h | 136 +++++ .../include/kr_sbus_interface/sbus_msg.h | 62 +++ .../kr_sbus_interface/sbus_serial_port.h | 47 ++ .../kr_sbus_interface/thrust_mapping.h | 39 ++ .../kr_sbus_interface/launch/test.launch | 18 + .../kr_sbus_interface/msg/SbusRosMessage.msg | 14 + .../kr_sbus_interface/nodelet_plugin.xml | 7 + interfaces/kr_sbus_interface/package.xml | 21 + .../kr_sbus_interface/src/sbus_bridge.cpp | 516 ++++++++++++++++++ interfaces/kr_sbus_interface/src/sbus_msg.cpp | 129 +++++ .../src/sbus_serial_port.cpp | 402 ++++++++++++++ .../src/so3cmd_to_sbus_nodelet.cpp | 197 +++++++ .../kr_sbus_interface/src/thrust_mapping.cpp | 76 +++ 20 files changed, 1810 insertions(+) create mode 100644 interfaces/kr_sbus_interface/CMakeLists.txt create mode 100644 interfaces/kr_sbus_interface/cmake/FindSnav.cmake create mode 100644 interfaces/kr_sbus_interface/config/gains.yaml create mode 100644 interfaces/kr_sbus_interface/config/mav_manager_params.yaml create mode 100644 interfaces/kr_sbus_interface/config/tracker_params.yaml create mode 100644 interfaces/kr_sbus_interface/config/trackers.yaml create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h create mode 100644 interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h create mode 100644 interfaces/kr_sbus_interface/launch/test.launch create mode 100644 interfaces/kr_sbus_interface/msg/SbusRosMessage.msg create mode 100644 interfaces/kr_sbus_interface/nodelet_plugin.xml create mode 100644 interfaces/kr_sbus_interface/package.xml create mode 100644 interfaces/kr_sbus_interface/src/sbus_bridge.cpp create mode 100644 interfaces/kr_sbus_interface/src/sbus_msg.cpp create mode 100644 interfaces/kr_sbus_interface/src/sbus_serial_port.cpp create mode 100644 interfaces/kr_sbus_interface/src/so3cmd_to_sbus_nodelet.cpp create mode 100644 interfaces/kr_sbus_interface/src/thrust_mapping.cpp diff --git a/interfaces/kr_sbus_interface/CMakeLists.txt b/interfaces/kr_sbus_interface/CMakeLists.txt new file mode 100644 index 00000000..e227738a --- /dev/null +++ b/interfaces/kr_sbus_interface/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.10) +project(kr_sbus_interface) + +# set default build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE RelWithDebInfo) +endif() + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +add_compile_options(-Wall) + +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/") + +find_package( + catkin REQUIRED + COMPONENTS roscpp + kr_mav_msgs + nav_msgs + nodelet + tf_conversions) +find_package(Eigen3 REQUIRED) + +include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) + +catkin_package( +LIBRARIES +${PROJECT_NAME} +CATKIN_DEPENDS +roscpp +kr_mav_msgs +nav_msgs +nodelet +tf_conversions +DEPENDS +EIGEN3) + +add_library( + ${PROJECT_NAME} + src/so3cmd_to_sbus_nodelet.cpp + src/sbus_bridge.cpp + src/sbus_msg.cpp + src/sbus_serial_port.cpp + src/thrust_mapping.cpp) +add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) +install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) +install(FILES nodelet_plugin.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/interfaces/kr_sbus_interface/cmake/FindSnav.cmake b/interfaces/kr_sbus_interface/cmake/FindSnav.cmake new file mode 100644 index 00000000..07f3cd69 --- /dev/null +++ b/interfaces/kr_sbus_interface/cmake/FindSnav.cmake @@ -0,0 +1,21 @@ +FIND_PATH(Snav_INCLUDE_DIR + NAMES + snav/snapdragon_navigator.h + PATHS + /usr/include + NO_DEFAULT_PATH +) + +FIND_LIBRARY(Snav_LIBRARY + NAMES + snav_arm + PATHS + /usr/lib + NO_DEFAULT_PATH +) + +IF(Snav_INCLUDE_DIR AND Snav_LIBRARY) + SET(Snav_FOUND TRUE) +ELSE(Snav_INCLUDE_DIR AND Snav_LIBRARY) + SET(Snav_FOUND FALSE) +ENDIF(Snav_INCLUDE_DIR AND Snav_LIBRARY) diff --git a/interfaces/kr_sbus_interface/config/gains.yaml b/interfaces/kr_sbus_interface/config/gains.yaml new file mode 100644 index 00000000..0c57f8a8 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/gains.yaml @@ -0,0 +1,15 @@ +gains: + pos: {x: 3.4, y: 3.4, z: 5.4} + vel: {x: 2.8, y: 2.8, z: 3.0} + ki: {x: 0.00, y: 0.00, z: 0.00, yaw: 0.01} + kib: {x: 0.00, y: 0.00, z: 0.00} + rot: {x: 1.5, y: 1.5, z: 1.0} + ang: {x: 0.13, y: 0.13, z: 0.1} + +corrections: + kf: 0.0e-08 + r: 0.0 + p: 0.0 + +max_pos_int: 0.5 +max_pos_int_b: 0.5 \ No newline at end of file diff --git a/interfaces/kr_sbus_interface/config/mav_manager_params.yaml b/interfaces/kr_sbus_interface/config/mav_manager_params.yaml new file mode 100644 index 00000000..c0c3c823 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/mav_manager_params.yaml @@ -0,0 +1,6 @@ +server_wait_timeout: 0.5 +need_imu: false +need_output_data: true +use_attitude_safety_catch: true +max_attitude_angle: 0.43 +takeoff_height: 0.2 diff --git a/interfaces/kr_sbus_interface/config/tracker_params.yaml b/interfaces/kr_sbus_interface/config/tracker_params.yaml new file mode 100644 index 00000000..fd97a0b0 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/tracker_params.yaml @@ -0,0 +1,25 @@ +# This should contain tracker parameters + +line_tracker_distance: + default_v_des: 1.0 + default_a_des: 0.5 + epsilon: 0.1 + +line_tracker_min_jerk: + default_v_des: 1.0 + default_a_des: 0.5 + default_yaw_v_des: 0.8 + default_yaw_a_des: 0.2 + +trajectory_tracker: + max_vel_des: 0.5 + max_acc_des: 0.3 + +velocity_tracker: + timeout: 0.5 + +lissajous_tracker: + frame_id: odom + +lissajous_adder: + frame_id: odom \ No newline at end of file diff --git a/interfaces/kr_sbus_interface/config/trackers.yaml b/interfaces/kr_sbus_interface/config/trackers.yaml new file mode 100644 index 00000000..694f6177 --- /dev/null +++ b/interfaces/kr_sbus_interface/config/trackers.yaml @@ -0,0 +1,11 @@ +trackers: + - kr_trackers/LineTrackerMinJerk + - kr_trackers/LineTrackerDistance +# - kr_trackers/LineTrackerTrapezoid +# - kr_trackers/LineTrackerYaw + - kr_trackers/VelocityTrackerAction + - kr_trackers/NullTracker + - kr_trackers/CircleTracker + - kr_trackers/TrajectoryTracker + - kr_trackers/SmoothVelTracker + - kr_trackers/LissajousTracker diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h new file mode 100644 index 00000000..d0a85ecb --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/channel_mapping.h @@ -0,0 +1,17 @@ +#pragma once + +namespace sbus_bridge { + +namespace channel_mapping { + +static constexpr uint8_t kThrottle = 0; +static constexpr uint8_t kRoll = 1; +static constexpr uint8_t kPitch = 2; +static constexpr uint8_t kYaw = 3; +static constexpr uint8_t kArming = 4; +static constexpr uint8_t kControlMode = 5; +static constexpr uint8_t kGamepadMode = 6; + +} // namespace channel_mapping + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h new file mode 100644 index 00000000..e2b48de5 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_bridge.h @@ -0,0 +1,136 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace sbus_bridge { + +enum class BridgeState { OFF, ARMING, AUTONOMOUS_FLIGHT, RC_FLIGHT }; + +class SBusBridge : public SBusSerialPort { + public: + SBusBridge(const ros::NodeHandle& nh, const ros::NodeHandle& pnh); + + SBusBridge() : SBusBridge(ros::NodeHandle(), ros::NodeHandle("~")) {} + + void controlCommandCallback(const kr_mav_msgs::SO3Command::ConstPtr& msg, const Eigen::Quaterniond& odom_q); + + virtual ~SBusBridge(); + + private: + void watchdogThread(); + + void handleReceivedSbusMessage(const SBusMsg& received_sbus_msg) override; + + SBusMsg generateSBusMessageFromSO3Command(const kr_mav_msgs::SO3Command::ConstPtr& control_command, const Eigen::Quaterniond& odom_q) const; + + void sendSBusMessageToSerialPort(const SBusMsg& sbus_msg); + + void setBridgeState(const BridgeState& desired_bridge_state); + + void armBridgeCallback(const std_msgs::Bool::ConstPtr& msg); + void batteryVoltageCallback(const std_msgs::Float32::ConstPtr& msg); + void publishLowLevelFeedback(const ros::TimerEvent& time) const; + + bool loadParameters(); + + ros::NodeHandle nh_; + ros::NodeHandle pnh_; + + // Mutex for: + // - bridge_state_ + // - control_mode_ + // - bridge_armed_ + // - time_last_active_control_command_received_ + // - time_last_rc_msg_received_ + // - arming_counter_ + // - time_last_sbus_msg_sent_ + // Also "setBridgeState" and "sendSBusMessageToSerialPort" should only be + // called when "main_mutex_" is locked + mutable std::mutex main_mutex_; + // Mutex for: + // - battery_voltage_ + // - time_last_battery_voltage_received_ + // Also "generateSBusMessageFromControlCommand" should only be called when + // "battery_voltage_mutex_" is locked + mutable std::mutex battery_voltage_mutex_; + + // Publishers + ros::Publisher low_level_feedback_pub_; + ros::Publisher received_sbus_msg_pub_; + + // Subscribers + ros::Subscriber control_command_sub_; + ros::Subscriber arm_bridge_sub_; + ros::Subscriber battery_voltage_sub_; + + // Timer + ros::Timer low_level_feedback_pub_timer_; + + // Watchdog + std::thread watchdog_thread_; + std::atomic_bool stop_watchdog_thread_; + ros::Time time_last_rc_msg_received_; + ros::Time time_last_sbus_msg_sent_; + ros::Time time_last_battery_voltage_received_; + ros::Time time_last_active_control_command_received_; + + BridgeState bridge_state_; + ControlMode control_mode_; + int arming_counter_; + double battery_voltage_; + + // Safety flags + bool bridge_armed_; + bool rc_was_disarmed_once_; + + std::atomic_bool destructor_invoked_; + + thrust_mapping::CollectiveThrustMapping thrust_mapping_; + + // Parameters + std::string port_name_; + bool enable_receiving_sbus_messages_; + + double control_command_timeout_; + double rc_timeout_; + + double mass_; + + bool disable_thrust_mapping_; + + double max_roll_rate_; + double max_pitch_rate_; + double max_yaw_rate_; + + double max_roll_angle_; + double max_pitch_angle_; + + double alpha_vbat_filter_; + bool perform_thrust_voltage_compensation_; + int n_lipo_cells_; + + // Constants + static constexpr double kLowLevelFeedbackPublishFrequency_ = 50.0; + + static constexpr int kSmoothingFailRepetitions_ = 5; + + static constexpr double kBatteryLowVoltagePerCell_ = 3.6; + static constexpr double kBatteryCriticalVoltagePerCell_ = 3.4; + static constexpr double kBatteryInvalidVoltagePerCell_ = 3.0; + static constexpr double kBatteryVoltageTimeout_ = 1.0; +}; + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h new file mode 100644 index 00000000..bf27e294 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_msg.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +// #include "kr_sbus_interface/SbusRosMessage.h" +# include + +namespace sbus_bridge { + +enum class ControlMode { NONE, ATTITUDE, BODY_RATES }; + +enum class ArmState { DISARMED, ARMED }; + +#pragma pack(push) +#pragma pack(1) +struct SBusMsg { + // Constants + static constexpr int kNChannels = 16; + static constexpr uint16_t kMinCmd = 192; // corresponds to 1000 on FC + static constexpr uint16_t kMeanCmd = 992; // corresponds to 1500 on FC + static constexpr uint16_t kMaxCmd = 1792; // corresponds to 2000 on FC + + ros::Time timestamp; + + // Normal 11 bit channels + uint16_t channels[kNChannels]; + + // Digital channels (ch17 and ch18) + bool digital_channel_1; + bool digital_channel_2; + + // Flags + bool frame_lost; + bool failsafe; + + SBusMsg(); +// SBusMsg(const sbus_bridge::SbusRosMessage& sbus_ros_msg); + virtual ~SBusMsg(); + +// sbus_bridge::SbusRosMessage toRosMessage() const; + + void limitAllChannelsFeasible(); + void limitSbusChannelFeasible(const int channel_idx); + + // Setting sbus command helpers + void setThrottleCommand(const uint16_t throttle_cmd); + void setRollCommand(const uint16_t roll_cmd); + void setPitchCommand(const uint16_t pitch_cmd); + void setYawCommand(const uint16_t yaw_cmd); + void setControlMode(const ControlMode& control_mode); + void setControlModeAttitude(); + void setControlModeBodyRates(); + void setArmState(const ArmState& arm_state); + void setArmStateArmed(); + void setArmStateDisarmed(); + + // Sbus message check helpers + bool isArmed() const; + ControlMode getControlMode() const; +}; +#pragma pack(pop) +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h new file mode 100644 index 00000000..fb64614d --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/sbus_serial_port.h @@ -0,0 +1,47 @@ +#pragma once + +#include +#include + +#include "kr_sbus_interface/sbus_msg.h" + +namespace sbus_bridge { + +class SBusSerialPort { + public: + SBusSerialPort(); + SBusSerialPort(const std::string& port, const bool start_receiver_thread); + virtual ~SBusSerialPort(); + + protected: + bool setUpSBusSerialPort(const std::string& port, + const bool start_receiver_thread); + + bool connectSerialPort(const std::string& port); + void disconnectSerialPort(); + + bool startReceiverThread(); + bool stopReceiverThread(); + + void transmitSerialSBusMessage(const SBusMsg& sbus_msg) const; + virtual void handleReceivedSbusMessage( + const sbus_bridge::SBusMsg& received_sbus_msg) = 0; + + private: + static constexpr int kSbusFrameLength_ = 25; + static constexpr uint8_t kSbusHeaderByte_ = 0x0F; + static constexpr uint8_t kSbusFooterByte_ = 0x00; + static constexpr int kPollTimeoutMilliSeconds_ = 500; + + bool configureSerialPortForSBus() const; + void serialPortReceiveThread(); + sbus_bridge::SBusMsg parseSbusMessage( + uint8_t sbus_msg_bytes[kSbusFrameLength_]) const; + + std::thread receiver_thread_; + std::atomic_bool receiver_thread_should_exit_; + + int serial_port_fd_; +}; + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h b/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h new file mode 100644 index 00000000..9faed5a9 --- /dev/null +++ b/interfaces/kr_sbus_interface/include/kr_sbus_interface/thrust_mapping.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +namespace thrust_mapping { + +class CollectiveThrustMapping { + public: + CollectiveThrustMapping(); + CollectiveThrustMapping(const double thrust_map_a, const double thrust_map_b, + const double thrust_map_c, + const bool perform_thrust_voltage_compensation, + const double thrust_ratio_voltage_map_a, + const double thrust_ratio_voltage_map_b, + const int n_lipo_cells); + + virtual ~CollectiveThrustMapping(); + + uint16_t inverseThrustMapping(const double thrust, + const double battery_voltage) const; + + bool loadParameters(); + + private: + double thrust_map_a_; + double thrust_map_b_; + double thrust_map_c_; + + bool perform_thrust_voltage_compensation_; + double thrust_ratio_voltage_map_a_; + double thrust_ratio_voltage_map_b_; + int n_lipo_cells_; + + // Constants + static constexpr double kMinBatteryCompensationVoltagePerCell_ = 3.5; + static constexpr double kMaxBatteryCompensationVoltagePerCell_ = 4.2; +}; + +} // namespace thrust_mapping diff --git a/interfaces/kr_sbus_interface/launch/test.launch b/interfaces/kr_sbus_interface/launch/test.launch new file mode 100644 index 00000000..ab1dae90 --- /dev/null +++ b/interfaces/kr_sbus_interface/launch/test.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + diff --git a/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg b/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg new file mode 100644 index 00000000..3802e30f --- /dev/null +++ b/interfaces/kr_sbus_interface/msg/SbusRosMessage.msg @@ -0,0 +1,14 @@ +Header header + +# Regular 16 sbus channels with 11 bit value range each +uint16[16] channels + +# Digital channels +bool digital_channel_1 +bool digital_channel_2 + +# Frame lost flag +bool frame_lost + +# Failsafe flag +bool failsafe diff --git a/interfaces/kr_sbus_interface/nodelet_plugin.xml b/interfaces/kr_sbus_interface/nodelet_plugin.xml new file mode 100644 index 00000000..4c71b948 --- /dev/null +++ b/interfaces/kr_sbus_interface/nodelet_plugin.xml @@ -0,0 +1,7 @@ + + + + This take a so3_command and sends it to the robot using the SBUS Bridge + + + diff --git a/interfaces/kr_sbus_interface/package.xml b/interfaces/kr_sbus_interface/package.xml new file mode 100644 index 00000000..aa4e785d --- /dev/null +++ b/interfaces/kr_sbus_interface/package.xml @@ -0,0 +1,21 @@ + + + kr_sbus_interface + 0.0.0 + The kr_sbus_interface package + + Dexter Ong + + BSD + + catkin + message_generation + nav_msgs + nodelet + kr_mav_msgs + roscpp + tf_conversions + + + + diff --git a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp new file mode 100644 index 00000000..70415253 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp @@ -0,0 +1,516 @@ +#include + +#include + +#include + +namespace sbus_bridge { + +SBusBridge::SBusBridge(const ros::NodeHandle& nh, const ros::NodeHandle& pnh) + : nh_(nh), + pnh_(pnh), + stop_watchdog_thread_(false), + time_last_rc_msg_received_(), + time_last_sbus_msg_sent_(ros::Time::now()), + time_last_battery_voltage_received_(ros::Time::now()), + time_last_active_control_command_received_(), + bridge_state_(BridgeState::OFF), + control_mode_(ControlMode::NONE), + arming_counter_(0), + battery_voltage_(0.0), + bridge_armed_(false), + rc_was_disarmed_once_(false), + destructor_invoked_(false) { +// if (!loadParameters()) { +// ROS_ERROR("[%s] Could not load parameters.", pnh_.getNamespace().c_str()); +// ros::shutdown(); +// return; +// } + + if (disable_thrust_mapping_) { + ROS_WARN("[%s] Thrust mapping disabled!", pnh_.getNamespace().c_str()); + } + +// // Publishers +// low_level_feedback_pub_ = +// nh_.advertise("low_level_feedback", 1); +// if (enable_receiving_sbus_messages_) { +// received_sbus_msg_pub_ = +// nh_.advertise("received_sbus_message", 1); +// } + +// // Subscribers +// arm_bridge_sub_ = +// nh_.subscribe("sbus_bridge/arm", 1, &SBusBridge::armBridgeCallback, this); +// control_command_sub_ = nh_.subscribe( +// "control_command", 1, &SBusBridge::controlCommandCallback, this); +// battery_voltage_sub_ = nh_.subscribe( +// "battery_voltage", 1, &SBusBridge::batteryVoltageCallback, this); + +// low_level_feedback_pub_timer_ = +// nh_.createTimer(ros::Duration(1.0 / kLowLevelFeedbackPublishFrequency_), +// &SBusBridge::publishLowLevelFeedback, this); + + // Start serial port with receiver thread if receiving sbus messages is + // enabled + if (!setUpSBusSerialPort(port_name_, enable_receiving_sbus_messages_)) { + ros::shutdown(); + return; + } + + // Start watchdog thread + try { + watchdog_thread_ = std::thread(&SBusBridge::watchdogThread, this); + } catch (...) { + ROS_ERROR("[%s] Could not successfully start watchdog thread.", + pnh_.getNamespace().c_str()); + ros::shutdown(); + return; + } +} + +SBusBridge::~SBusBridge() { + destructor_invoked_ = true; + + // Stop SBus receiver thread + if (enable_receiving_sbus_messages_) { + stopReceiverThread(); + } + + // Stop watchdog thread + stop_watchdog_thread_ = true; + // Wait for watchdog thread to finish + watchdog_thread_.join(); + + // Now only one thread (the ROS thread) is remaining + + setBridgeState(BridgeState::OFF); + + // Send disarming SBus message for safety + // We repeat it to prevent any possible smoothing of commands on the flight + // controller to interfere with this + SBusMsg shut_down_message; + shut_down_message.setArmStateDisarmed(); + ros::Rate loop_rate(110.0); + for (int i = 0; i < kSmoothingFailRepetitions_; i++) { + transmitSerialSBusMessage(shut_down_message); + loop_rate.sleep(); + } + + // Close serial port + disconnectSerialPort(); +} + +void SBusBridge::watchdogThread() { + ros::Rate watchdog_rate(110.0); + while (ros::ok() && !stop_watchdog_thread_) { + watchdog_rate.sleep(); + + std::lock_guard main_lock(main_mutex_); + + const ros::Time time_now = ros::Time::now(); + + if (bridge_state_ == BridgeState::RC_FLIGHT && + time_now - time_last_rc_msg_received_ > ros::Duration(rc_timeout_)) { + // If the last received RC command was armed but was received longer than + // rc_timeout ago we switch the bridge state to AUTONOMOUS_FLIGHT. + // In case there are no valid control commands the bridge state is set to + // OFF in the next check below + ROS_WARN( + "[%s] Remote control was active but no message from it was received " + "within timeout (%f s).", + pnh_.getNamespace().c_str(), rc_timeout_); + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } + + if (bridge_state_ == BridgeState::ARMING || + bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) { + if (time_now - time_last_active_control_command_received_ > + ros::Duration(control_command_timeout_)) { + // When switching the bridge state to off, our watchdog ensures that a + // disarming off message is repeated. + setBridgeState(BridgeState::OFF); + // Note: Control could theoretically still be taken over by RC but if + // this happened in flight it might require super human reaction since + // in this case the quad can not be armed with non zero throttle by + // the remote. + } + } + + if (bridge_state_ == BridgeState::OFF) { + // Send off message that disarms the vehicle + // We repeat it to prevent any weird behavior that occurs if the flight + // controller is not receiving commands for a while + SBusMsg off_msg; + off_msg.setArmStateDisarmed(); + sendSBusMessageToSerialPort(off_msg); + } + + // Check battery voltage timeout + std::lock_guard battery_lock(battery_voltage_mutex_); + if (time_now - time_last_battery_voltage_received_ > + ros::Duration(kBatteryVoltageTimeout_)) { + battery_voltage_ = 0.0; + if (perform_thrust_voltage_compensation_) { + ROS_WARN_THROTTLE( + 1.0, + "[%s] Can not perform battery voltage compensation because there " + "is no recent battery voltage measurement", + pnh_.getNamespace().c_str()); + } + } + + // Mutexes are unlocked because they go out of scope here + } +} + +void SBusBridge::handleReceivedSbusMessage(const SBusMsg& received_sbus_msg) +{ +// { +// std::lock_guard main_lock(main_mutex_); + +// time_last_rc_msg_received_ = ros::Time::now(); + +// if (received_sbus_msg.isArmed()) { +// if (!rc_was_disarmed_once_) { +// // This flag prevents that the vehicle can be armed if the RC is armed +// // on startup of the bridge +// ROS_WARN_THROTTLE( +// 1.0, +// "[%s] RC needs to be disarmed once before it can take over control", +// pnh_.getNamespace().c_str()); +// return; +// } + +// // Immediately go into RC_FLIGHT state since RC always has priority +// if (bridge_state_ != BridgeState::RC_FLIGHT) { +// setBridgeState(BridgeState::RC_FLIGHT); +// ROS_INFO("[%s] Control authority taken over by remote control.", +// pnh_.getNamespace().c_str()); +// } +// sendSBusMessageToSerialPort(received_sbus_msg); +// control_mode_ = received_sbus_msg.getControlMode(); +// } else if (bridge_state_ == BridgeState::RC_FLIGHT) { +// // If the bridge was in state RC_FLIGHT and the RC is disarmed we set the +// // state to AUTONOMOUS_FLIGHT +// // In case there are valid control commands, the bridge will stay in +// // AUTONOMOUS_FLIGHT, otherwise the watchdog will set the state to OFF +// ROS_INFO("[%s] Control authority returned by remote control.", +// pnh_.getNamespace().c_str()); +// if (bridge_armed_) { +// setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); +// } else { +// // When switching the bridge state to off, our watchdog ensures that a +// // disarming off message is sent +// setBridgeState(BridgeState::OFF); +// } +// } else if (!rc_was_disarmed_once_) { +// ROS_INFO( +// "[%s] RC was disarmed once, now it is allowed to take over control", +// pnh_.getNamespace().c_str()); +// rc_was_disarmed_once_ = true; +// } + +// // Main mutex is unlocked here because it goes out of scope +// } + +// received_sbus_msg_pub_.publish(received_sbus_msg.toRosMessage()); +} + +void SBusBridge::controlCommandCallback( + const kr_mav_msgs::SO3Command::ConstPtr& msg, + const Eigen::Quaterniond& odom_q) +{ + std::lock_guard main_lock(main_mutex_); + + if (destructor_invoked_) { + // This ensures that if the destructor was invoked we do not try to write + // to the serial port anymore because of receiving a control command + return; + } + + if (!bridge_armed_ || bridge_state_ == BridgeState::RC_FLIGHT) { + // If bridge is not armed we do not allow control commands to be sent + // RC has priority over control commands for autonomous flying + if (!bridge_armed_ && msg->aux.enable_motors && + bridge_state_ != BridgeState::RC_FLIGHT) + { + ROS_WARN_THROTTLE( + 1.0, + "[%s] Received active control command but sbus bridge is not armed.", + pnh_.getNamespace().c_str()); + } + return; + } + + SBusMsg sbus_msg_to_send; + { + std::lock_guard battery_lock(battery_voltage_mutex_); + // Set commands + sbus_msg_to_send = generateSBusMessageFromSO3Command(msg, odom_q); + // Battery voltage mutex is unlocked because it goes out of scope here + } + + // Send disarm if necessary + if (!msg->aux.enable_motors) + { + if (bridge_state_ == BridgeState::ARMING || + bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) + { + setBridgeState(BridgeState::OFF); + } + // Make sure vehicle is disarmed to immediately switch it off + sbus_msg_to_send.setArmStateDisarmed(); + } + + // Limit channels + sbus_msg_to_send.limitAllChannelsFeasible(); + + // Immediately send SBus message + sendSBusMessageToSerialPort(sbus_msg_to_send); + + // Main mutex is unlocked because it goes out of scope here +} + +void SBusBridge::sendSBusMessageToSerialPort(const SBusMsg& sbus_msg) +{ + std::cout << "sent" << std::endl; + SBusMsg sbus_message_to_send = sbus_msg; + + switch (bridge_state_) { + case BridgeState::OFF: + // Disarm vehicle + sbus_message_to_send.setArmStateDisarmed(); + break; + + case BridgeState::ARMING: + // Since there might be some RC commands smoothing and checks on multiple + // messages before arming, we repeat the messages with minimum throttle + // and arming command multiple times. 5 times seems to work robustly. + if (arming_counter_ >= kSmoothingFailRepetitions_) { + setBridgeState(BridgeState::AUTONOMOUS_FLIGHT); + } else { + // Set thrust to minimum command to make sure FC arms + sbus_message_to_send.setThrottleCommand(SBusMsg::kMinCmd); + sbus_message_to_send.setArmStateArmed(); + arming_counter_++; + } + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + sbus_message_to_send.setArmStateArmed(); + break; + + case BridgeState::RC_FLIGHT: + // Passing RC command straight through + break; + + default: + // Disarm the vehicle because this code must be terribly wrong + sbus_message_to_send.setArmStateDisarmed(); + ROS_WARN("[%s] Bridge is in unknown state, vehicle will be disarmed", + pnh_.getNamespace().c_str()); + break; + } + + if ((ros::Time::now() - time_last_sbus_msg_sent_).toSec() <= 0.006) { + // An SBUS message takes 3ms to be transmitted by the serial port so let's + // not stress it too much. This should only happen in case of switching + // between control commands and rc commands + if (bridge_state_ == BridgeState::ARMING) { + // In case of arming we want to send kSmoothingFailRepetitions_ messages + // with minimum throttle to the flight controller. Since this check + // prevents the message from being sent out we reduce the counter that + // was incremented above assuming the message would actually be sent. + arming_counter_--; + } + return; + } + + sbus_message_to_send.timestamp = ros::Time::now(); + transmitSerialSBusMessage(sbus_message_to_send); + time_last_sbus_msg_sent_ = ros::Time::now(); +} + +SBusMsg SBusBridge::generateSBusMessageFromSO3Command(const kr_mav_msgs::SO3Command::ConstPtr& msg, const Eigen::Quaterniond& odom_q) const +{ + std::cout << "generating message" << std::endl; + SBusMsg sbus_msg; + + sbus_msg.setArmStateArmed(); + + // grab desired forces and rotation from so3 + const Eigen::Vector3d f_des(msg->force.x, msg->force.y, msg->force.z); + + const Eigen::Quaterniond q_des(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); + // const Eigen::Vector3d ang_vel(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z); + + // convert to tf::Quaternion + // tf::Quaternion imu_tf = tf::Quaternion(imu_q_.x(), imu_q_.y(), imu_q_.z(), imu_q_.w()); + // tf::Quaternion odom_tf = tf::Quaternion(odom_q_.x(), odom_q_.y(), odom_q_.z(), odom_q_.w()); + + const Eigen::Matrix3d R_cur(odom_q); + + double thrust = f_des(0) * R_cur(0, 2) + f_des(1) * R_cur(1, 2) + f_des(2) * R_cur(2, 2); + + sbus_msg.setThrottleCommand(thrust_mapping_.inverseThrustMapping( + thrust * mass_, battery_voltage_)); + + Eigen::Vector3d desired_euler_angles = q_des.toRotationMatrix().eulerAngles(0, 1, 2); + + sbus_msg.setRollCommand( + round((desired_euler_angles(0) / max_roll_angle_) * + (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + + SBusMsg::kMeanCmd)); + sbus_msg.setPitchCommand( + round((desired_euler_angles(1) / max_pitch_angle_) * + (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + + SBusMsg::kMeanCmd)); + sbus_msg.setYawCommand( + round((msg->angular_velocity.z / max_yaw_rate_) * + (SBusMsg::kMaxCmd - SBusMsg::kMeanCmd) + + SBusMsg::kMeanCmd)); + + return sbus_msg; +} + +void SBusBridge::setBridgeState(const BridgeState& desired_bridge_state) { + switch (desired_bridge_state) { + case BridgeState::OFF: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::ARMING: + bridge_state_ = desired_bridge_state; + arming_counter_ = 0; + break; + + case BridgeState::AUTONOMOUS_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + case BridgeState::RC_FLIGHT: + bridge_state_ = desired_bridge_state; + break; + + default: + ROS_WARN("[%s] Wanted to switch to unknown bridge state", + pnh_.getNamespace().c_str()); + } +} + +void SBusBridge::armBridgeCallback(const std_msgs::Bool::ConstPtr& msg) { + std::lock_guard main_lock(main_mutex_); + + if (destructor_invoked_) { + // On shut down we do not allow to arm (or disarm) the bridge anymore + return; + } + + if (msg->data) { + bridge_armed_ = true; + ROS_INFO("[%s] Bridge armed", pnh_.getNamespace().c_str()); + } else { + bridge_armed_ = false; + if (bridge_state_ == BridgeState::ARMING || + bridge_state_ == BridgeState::AUTONOMOUS_FLIGHT) { + setBridgeState(BridgeState::OFF); + } + ROS_INFO("[%s] Bridge disarmed", pnh_.getNamespace().c_str()); + } +} + +void SBusBridge::batteryVoltageCallback( + const std_msgs::Float32::ConstPtr& msg) { + std::lock_guard battery_lock(battery_voltage_mutex_); + + if (battery_voltage_ != 0.0) { + battery_voltage_ = alpha_vbat_filter_ * msg->data + + (1.0 - alpha_vbat_filter_) * battery_voltage_; + } else { + battery_voltage_ = msg->data; + } + time_last_battery_voltage_received_ = ros::Time::now(); +} + +void SBusBridge::publishLowLevelFeedback(const ros::TimerEvent& time) const { +// quadrotor_msgs::LowLevelFeedback low_level_feedback_msg; + +// { +// std::lock_guard main_lock(main_mutex_); +// std::lock_guard battery_lock(battery_voltage_mutex_); + +// // Publish a low level feedback message +// low_level_feedback_msg.header.stamp = ros::Time::now(); +// low_level_feedback_msg.battery_voltage = battery_voltage_; +// if (battery_voltage_ > n_lipo_cells_ * kBatteryLowVoltagePerCell_) { +// low_level_feedback_msg.battery_state = low_level_feedback_msg.BAT_GOOD; +// } else if (battery_voltage_ > +// n_lipo_cells_ * kBatteryCriticalVoltagePerCell_) { +// low_level_feedback_msg.battery_state = low_level_feedback_msg.BAT_LOW; +// } else if (battery_voltage_ > +// n_lipo_cells_ * kBatteryInvalidVoltagePerCell_) { +// low_level_feedback_msg.battery_state = +// low_level_feedback_msg.BAT_CRITICAL; +// } else { +// low_level_feedback_msg.battery_state = low_level_feedback_msg.BAT_INVALID; +// } + +// if (bridge_state_ == BridgeState::RC_FLIGHT) { +// low_level_feedback_msg.control_mode = low_level_feedback_msg.RC_MANUAL; +// } else { +// if (control_mode_ == ControlMode::ATTITUDE) { +// low_level_feedback_msg.control_mode = low_level_feedback_msg.ATTITUDE; +// } else if (control_mode_ == ControlMode::BODY_RATES) { +// low_level_feedback_msg.control_mode = low_level_feedback_msg.BODY_RATES; +// } else { +// low_level_feedback_msg.control_mode = low_level_feedback_msg.NONE; +// } +// } + +// // Mutexes are unlocked here since they go out of scope +// } + +// low_level_feedback_pub_.publish(low_level_feedback_msg); +} + +// bool SBusBridge::loadParameters() { +// #define GET_PARAM(name) \ +// if (!quadrotor_common::getParam(#name, name##_, pnh_)) return false + +// GET_PARAM(port_name); +// GET_PARAM(enable_receiving_sbus_messages); + +// GET_PARAM(control_command_timeout); +// GET_PARAM(rc_timeout); + +// GET_PARAM(mass); + +// GET_PARAM(disable_thrust_mapping); + +// GET_PARAM(max_roll_rate); +// GET_PARAM(max_pitch_rate); +// GET_PARAM(max_yaw_rate); +// max_roll_rate_ /= (180.0 / M_PI); +// max_pitch_rate_ /= (180.0 / M_PI); +// max_yaw_rate_ /= (180.0 / M_PI); + +// GET_PARAM(max_roll_angle); +// GET_PARAM(max_pitch_angle); +// max_roll_angle_ /= (180.0 / M_PI); +// max_pitch_angle_ /= (180.0 / M_PI); + +// GET_PARAM(alpha_vbat_filter); +// GET_PARAM(perform_thrust_voltage_compensation); +// GET_PARAM(n_lipo_cells); + +// if (!thrust_mapping_.loadParameters()) { +// return false; +// } + +// return true; + +// #undef GET_PARAM +// } + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/sbus_msg.cpp b/interfaces/kr_sbus_interface/src/sbus_msg.cpp new file mode 100644 index 00000000..4d124913 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/sbus_msg.cpp @@ -0,0 +1,129 @@ +#include "kr_sbus_interface/sbus_msg.h" + +#include "kr_sbus_interface/channel_mapping.h" + +namespace sbus_bridge { + +SBusMsg::SBusMsg() + : timestamp(ros::Time::now()), + digital_channel_1(false), + digital_channel_2(false), + frame_lost(false), + failsafe(false) { + for (int i = 0; i < kNChannels; i++) { + channels[i] = kMeanCmd; + } +} + +// SBusMsg::SBusMsg(const sbus_bridge::SbusRosMessage& sbus_ros_msg) { +// timestamp = sbus_ros_msg.header.stamp; +// for (uint8_t i; i < kNChannels; i++) { +// channels[i] = sbus_ros_msg.channels[i]; +// } +// digital_channel_1 = sbus_ros_msg.digital_channel_1; +// digital_channel_2 = sbus_ros_msg.digital_channel_2; +// frame_lost = sbus_ros_msg.frame_lost; +// failsafe = sbus_ros_msg.failsafe; +// } + +SBusMsg::~SBusMsg() {} + +// sbus_bridge::SbusRosMessage SBusMsg::toRosMessage() const { +// sbus_bridge::SbusRosMessage sbus_ros_msg; +// sbus_ros_msg.header.stamp = timestamp; +// for (uint8_t i; i < kNChannels; i++) { +// sbus_ros_msg.channels[i] = channels[i]; +// } +// sbus_ros_msg.digital_channel_1 = digital_channel_1; +// sbus_ros_msg.digital_channel_2 = digital_channel_2; +// sbus_ros_msg.frame_lost = frame_lost; +// sbus_ros_msg.failsafe = failsafe; + +// return sbus_ros_msg; +// } + +void SBusMsg::limitAllChannelsFeasible() { + for (uint8_t i = 0; i < kNChannels; i++) { + limitSbusChannelFeasible(i); + } +} + +void SBusMsg::limitSbusChannelFeasible(const int channel_idx) { + if (channel_idx < 0 || channel_idx >= kNChannels) { + return; + } + + if (channels[channel_idx] > kMaxCmd) { + channels[channel_idx] = kMaxCmd; + } + if (channels[channel_idx] < kMinCmd) { + channels[channel_idx] = kMinCmd; + } +} + +void SBusMsg::setThrottleCommand(const uint16_t throttle_cmd) { + channels[channel_mapping::kThrottle] = throttle_cmd; +} + +void SBusMsg::setRollCommand(const uint16_t roll_cmd) { + channels[channel_mapping::kRoll] = roll_cmd; +} + +void SBusMsg::setPitchCommand(const uint16_t pitch_cmd) { + channels[channel_mapping::kPitch] = pitch_cmd; +} + +void SBusMsg::setYawCommand(const uint16_t yaw_cmd) { + channels[channel_mapping::kYaw] = yaw_cmd; +} + +void SBusMsg::setControlMode(const ControlMode& control_mode) { + if (control_mode == ControlMode::ATTITUDE) { + channels[channel_mapping::kControlMode] = kMinCmd; + } else if (control_mode == ControlMode::BODY_RATES) { + channels[channel_mapping::kControlMode] = kMaxCmd; + } +} + +void SBusMsg::setControlModeAttitude() { + setControlMode(ControlMode::ATTITUDE); +} + +void SBusMsg::setControlModeBodyRates() { + setControlMode(ControlMode::BODY_RATES); +} + +void SBusMsg::setArmState(const ArmState& arm_state) { + if (arm_state == ArmState::ARMED) { + channels[channel_mapping::kArming] = kMaxCmd; + } else { + channels[channel_mapping::kArming] = kMinCmd; + } +} + +void SBusMsg::setArmStateArmed() { setArmState(ArmState::ARMED); } + +void SBusMsg::setArmStateDisarmed() { + setArmState(ArmState::DISARMED); + // Should not be necessary but for safety we also set the throttle command + // to the minimum + setThrottleCommand(kMinCmd); +} + +bool SBusMsg::isArmed() const { + if (channels[channel_mapping::kArming] <= kMeanCmd) { + return false; + } + + return true; +} + +ControlMode SBusMsg::getControlMode() const { + if (channels[channel_mapping::kControlMode] > kMeanCmd) { + return ControlMode::BODY_RATES; + } + + return ControlMode::ATTITUDE; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp b/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp new file mode 100644 index 00000000..54baaf11 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/sbus_serial_port.cpp @@ -0,0 +1,402 @@ +#include "kr_sbus_interface/sbus_serial_port.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace sbus_bridge { + +SBusSerialPort::SBusSerialPort() + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1) {} + +SBusSerialPort::SBusSerialPort(const std::string& port, + const bool start_receiver_thread) + : receiver_thread_(), + receiver_thread_should_exit_(false), + serial_port_fd_(-1) { + setUpSBusSerialPort(port, start_receiver_thread); +} + +SBusSerialPort::~SBusSerialPort() { disconnectSerialPort(); } + +bool SBusSerialPort::setUpSBusSerialPort(const std::string& port, + const bool start_receiver_thread) { + if (!connectSerialPort(port)) { + return false; + } + + if (start_receiver_thread) { + if (!startReceiverThread()) { + return false; + } + } + + return true; +} + +bool SBusSerialPort::connectSerialPort(const std::string& port) { + // Open serial port + // O_RDWR - Read and write + // O_NOCTTY - Ignore special chars like CTRL-C + serial_port_fd_ = open(port.c_str(), O_RDWR | O_NOCTTY); + ROS_INFO("[%s] Connect to serial port", ros::this_node::getName().c_str()); + + if (serial_port_fd_ == -1) { + ROS_ERROR("[%s] Could not open serial port %s", + ros::this_node::getName().c_str(), port.c_str()); + return false; + } + + if (!configureSerialPortForSBus()) { + close(serial_port_fd_); + ROS_ERROR("[%s] Could not set necessary configuration of serial port", + ros::this_node::getName().c_str()); + return false; + } + + return true; +} + +void SBusSerialPort::disconnectSerialPort() { + stopReceiverThread(); + + close(serial_port_fd_); +} + +bool SBusSerialPort::startReceiverThread() { + // Start watchdog thread + try { + receiver_thread_ = + std::thread(&SBusSerialPort::serialPortReceiveThread, this); + } catch (...) { + ROS_ERROR("[%s] Could not successfully start SBUS receiver thread.", + ros::this_node::getName().c_str()); + return false; + } + + return true; +} + +bool SBusSerialPort::stopReceiverThread() { + if (!receiver_thread_.joinable()) { + return true; + } + + receiver_thread_should_exit_ = true; + + // Wait for receiver thread to finish + receiver_thread_.join(); + + return true; +} + +bool SBusSerialPort::configureSerialPortForSBus() const { + // clear config + fcntl(serial_port_fd_, F_SETFL, 0); + // read non blocking + fcntl(serial_port_fd_, F_SETFL, FNDELAY); + + struct termios2 uart_config; + /* Fill the struct for the new configuration */ + ioctl(serial_port_fd_, TCGETS2, &uart_config); + + // Output flags - Turn off output processing + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + + // Input flags - Turn off input processing + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= + ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // No line processing: + // echo off + // echo newline off + // canonical mode off, + // extended input processing off + // signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + // Turn off character processing + // Turn off odd parity + uart_config.c_cflag &= ~(CSIZE | PARODD | CBAUD); + + // Enable parity generation on output and parity checking for input. + uart_config.c_cflag |= PARENB; + // Set two stop bits, rather than one. + uart_config.c_cflag |= CSTOPB; + // No output processing, force 8 bit input + uart_config.c_cflag |= CS8; + // Enable a non standard baud rate + uart_config.c_cflag |= BOTHER; + + // Set custom baud rate of 100'000 bits/s necessary for sbus + const speed_t spd = 100000; + uart_config.c_ispeed = spd; + uart_config.c_ospeed = spd; + + if (ioctl(serial_port_fd_, TCSETS2, &uart_config) < 0) { + ROS_ERROR("[%s] could not set configuration of serial port", + ros::this_node::getName().c_str()); + return false; + } + + return true; +} + +void SBusSerialPort::transmitSerialSBusMessage(const SBusMsg& sbus_msg) const { + static uint8_t buffer[kSbusFrameLength_]; + + // SBUS header + buffer[0] = kSbusHeaderByte_; + + // 16 channels of 11 bit data + buffer[1] = (uint8_t)((sbus_msg.channels[0] & 0x07FF)); + buffer[2] = (uint8_t)((sbus_msg.channels[0] & 0x07FF) >> 8 | + (sbus_msg.channels[1] & 0x07FF) << 3); + buffer[3] = (uint8_t)((sbus_msg.channels[1] & 0x07FF) >> 5 | + (sbus_msg.channels[2] & 0x07FF) << 6); + buffer[4] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 2); + buffer[5] = (uint8_t)((sbus_msg.channels[2] & 0x07FF) >> 10 | + (sbus_msg.channels[3] & 0x07FF) << 1); + buffer[6] = (uint8_t)((sbus_msg.channels[3] & 0x07FF) >> 7 | + (sbus_msg.channels[4] & 0x07FF) << 4); + buffer[7] = (uint8_t)((sbus_msg.channels[4] & 0x07FF) >> 4 | + (sbus_msg.channels[5] & 0x07FF) << 7); + buffer[8] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 1); + buffer[9] = (uint8_t)((sbus_msg.channels[5] & 0x07FF) >> 9 | + (sbus_msg.channels[6] & 0x07FF) << 2); + buffer[10] = (uint8_t)((sbus_msg.channels[6] & 0x07FF) >> 6 | + (sbus_msg.channels[7] & 0x07FF) << 5); + buffer[11] = (uint8_t)((sbus_msg.channels[7] & 0x07FF) >> 3); + buffer[12] = (uint8_t)((sbus_msg.channels[8] & 0x07FF)); + buffer[13] = (uint8_t)((sbus_msg.channels[8] & 0x07FF) >> 8 | + (sbus_msg.channels[9] & 0x07FF) << 3); + buffer[14] = (uint8_t)((sbus_msg.channels[9] & 0x07FF) >> 5 | + (sbus_msg.channels[10] & 0x07FF) << 6); + buffer[15] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 2); + buffer[16] = (uint8_t)((sbus_msg.channels[10] & 0x07FF) >> 10 | + (sbus_msg.channels[11] & 0x07FF) << 1); + buffer[17] = (uint8_t)((sbus_msg.channels[11] & 0x07FF) >> 7 | + (sbus_msg.channels[12] & 0x07FF) << 4); + buffer[18] = (uint8_t)((sbus_msg.channels[12] & 0x07FF) >> 4 | + (sbus_msg.channels[13] & 0x07FF) << 7); + buffer[19] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 1); + buffer[20] = (uint8_t)((sbus_msg.channels[13] & 0x07FF) >> 9 | + (sbus_msg.channels[14] & 0x07FF) << 2); + buffer[21] = (uint8_t)((sbus_msg.channels[14] & 0x07FF) >> 6 | + (sbus_msg.channels[15] & 0x07FF) << 5); + buffer[22] = (uint8_t)((sbus_msg.channels[15] & 0x07FF) >> 3); + + // SBUS flags + // (bit0 = least significant bit) + // bit0 = ch17 = digital channel (0x01) + // bit1 = ch18 = digital channel (0x02) + // bit2 = Frame lost, equivalent red LED on receiver (0x04) + // bit3 = Failsafe activated (0x08) + // bit4 = n/a + // bit5 = n/a + // bit6 = n/a + // bit7 = n/a + buffer[23] = 0x00; + if (sbus_msg.digital_channel_1) { + buffer[23] |= 0x01; + } + if (sbus_msg.digital_channel_2) { + buffer[23] |= 0x02; + } + if (sbus_msg.frame_lost) { + buffer[23] |= 0x04; + } + if (sbus_msg.failsafe) { + buffer[23] |= 0x08; + } + + // SBUS footer + buffer[24] = kSbusFooterByte_; + + const int written = write(serial_port_fd_, (char*)buffer, kSbusFrameLength_); + // tcflush(serial_port_fd_, TCOFLUSH); // There were rumors that this might + // not work on Odroids... + if (written != kSbusFrameLength_) { + ROS_ERROR("[%s] Wrote %d bytes but should have written %d", + ros::this_node::getName().c_str(), written, kSbusFrameLength_); + } +} + +void SBusSerialPort::serialPortReceiveThread() { + struct pollfd fds[1]; + fds[0].fd = serial_port_fd_; + fds[0].events = POLLIN; + + uint8_t init_buf[10]; + while (read(serial_port_fd_, init_buf, sizeof(init_buf)) > 0) { + // On startup, as long as we receive something, we keep reading to ensure + // that the first byte of the first poll is the start of an SBUS message + // and not some arbitrary byte. + // This should help to get the framing in sync in the beginning. + usleep(100); + } + + std::deque bytes_buf; + + while (!receiver_thread_should_exit_) { + // Buffer to read bytes from serial port. We make it large enough to + // potentially contain 4 sbus messages but its actual size probably does + // not matter too much + uint8_t read_buf[4 * kSbusFrameLength_]; + + if (poll(fds, 1, kPollTimeoutMilliSeconds_) > 0) { + if (fds[0].revents & POLLIN) { + const ssize_t nread = read(serial_port_fd_, read_buf, sizeof(read_buf)); + + for (ssize_t i = 0; i < nread; i++) { + bytes_buf.push_back(read_buf[i]); + } + + bool valid_sbus_message_received = false; + uint8_t sbus_msg_bytes[kSbusFrameLength_]; + while (bytes_buf.size() >= kSbusFrameLength_) { + // Check if we have a potentially valid SBUS message + // A valid SBUS message must have to correct header and footer byte + // as well as zeros in the four most significant bytes of the flag + // byte (byte 23) + if (bytes_buf.front() == kSbusHeaderByte_ && + !(bytes_buf[kSbusFrameLength_ - 2] & 0xF0) && + bytes_buf[kSbusFrameLength_ - 1] == kSbusFooterByte_) { + for (uint8_t i = 0; i < kSbusFrameLength_; i++) { + sbus_msg_bytes[i] = bytes_buf.front(); + bytes_buf.pop_front(); + } + + valid_sbus_message_received = true; + } else { + // If it is not a valid SBUS message but has a correct header byte + // we need to pop it to prevent staying in this loop forever + bytes_buf.pop_front(); + ROS_WARN("[%s] SBUS message framing not in sync", + ros::this_node::getName().c_str()); + } + + // If not, pop front elements until we have a valid header byte + while (!bytes_buf.empty() && bytes_buf.front() != kSbusHeaderByte_) { + bytes_buf.pop_front(); + } + } + + if (valid_sbus_message_received) { + // Sometimes we read more than one sbus message at the same time + // By running the loop above for as long as possible before handling + // the received sbus message we achieve to only process the latest one + const SBusMsg received_sbus_msg = parseSbusMessage(sbus_msg_bytes); + handleReceivedSbusMessage(received_sbus_msg); + } + } + } + } + + return; +} + +SBusMsg SBusSerialPort::parseSbusMessage( + uint8_t sbus_msg_bytes[kSbusFrameLength_]) const { + SBusMsg sbus_msg; + + sbus_msg.timestamp = ros::Time::now(); + + // Decode the 16 regular channels + sbus_msg.channels[0] = + (((uint16_t)sbus_msg_bytes[1]) | ((uint16_t)sbus_msg_bytes[2] << 8)) & + 0x07FF; + sbus_msg.channels[1] = (((uint16_t)sbus_msg_bytes[2] >> 3) | + ((uint16_t)sbus_msg_bytes[3] << 5)) & + 0x07FF; + sbus_msg.channels[2] = + (((uint16_t)sbus_msg_bytes[3] >> 6) | ((uint16_t)sbus_msg_bytes[4] << 2) | + ((uint16_t)sbus_msg_bytes[5] << 10)) & + 0x07FF; + sbus_msg.channels[3] = (((uint16_t)sbus_msg_bytes[5] >> 1) | + ((uint16_t)sbus_msg_bytes[6] << 7)) & + 0x07FF; + sbus_msg.channels[4] = (((uint16_t)sbus_msg_bytes[6] >> 4) | + ((uint16_t)sbus_msg_bytes[7] << 4)) & + 0x07FF; + sbus_msg.channels[5] = + (((uint16_t)sbus_msg_bytes[7] >> 7) | ((uint16_t)sbus_msg_bytes[8] << 1) | + ((uint16_t)sbus_msg_bytes[9] << 9)) & + 0x07FF; + sbus_msg.channels[6] = (((uint16_t)sbus_msg_bytes[9] >> 2) | + ((uint16_t)sbus_msg_bytes[10] << 6)) & + 0x07FF; + sbus_msg.channels[7] = (((uint16_t)sbus_msg_bytes[10] >> 5) | + ((uint16_t)sbus_msg_bytes[11] << 3)) & + 0x07FF; + sbus_msg.channels[8] = + (((uint16_t)sbus_msg_bytes[12]) | ((uint16_t)sbus_msg_bytes[13] << 8)) & + 0x07FF; + sbus_msg.channels[9] = (((uint16_t)sbus_msg_bytes[13] >> 3) | + ((uint16_t)sbus_msg_bytes[14] << 5)) & + 0x07FF; + sbus_msg.channels[10] = (((uint16_t)sbus_msg_bytes[14] >> 6) | + ((uint16_t)sbus_msg_bytes[15] << 2) | + ((uint16_t)sbus_msg_bytes[16] << 10)) & + 0x07FF; + sbus_msg.channels[11] = (((uint16_t)sbus_msg_bytes[16] >> 1) | + ((uint16_t)sbus_msg_bytes[17] << 7)) & + 0x07FF; + sbus_msg.channels[12] = (((uint16_t)sbus_msg_bytes[17] >> 4) | + ((uint16_t)sbus_msg_bytes[18] << 4)) & + 0x07FF; + sbus_msg.channels[13] = (((uint16_t)sbus_msg_bytes[18] >> 7) | + ((uint16_t)sbus_msg_bytes[19] << 1) | + ((uint16_t)sbus_msg_bytes[20] << 9)) & + 0x07FF; + sbus_msg.channels[14] = (((uint16_t)sbus_msg_bytes[20] >> 2) | + ((uint16_t)sbus_msg_bytes[21] << 6)) & + 0x07FF; + sbus_msg.channels[15] = (((uint16_t)sbus_msg_bytes[21] >> 5) | + ((uint16_t)sbus_msg_bytes[22] << 3)) & + 0x07FF; + + // Decode flags from byte 23 (see comments in sendSBusMessage function) + // By default, flags are set to false in SBusMsg constructor, so we only need + // to set them true if necessary + if (sbus_msg_bytes[23] & (1 << 0)) { + sbus_msg.digital_channel_1 = true; + } + + if (sbus_msg_bytes[23] & (1 << 1)) { + sbus_msg.digital_channel_2 = true; + } + + if (sbus_msg_bytes[23] & (1 << 2)) { + sbus_msg.frame_lost = true; + } + + if (sbus_msg_bytes[23] & (1 << 3)) { + sbus_msg.failsafe = true; + } + + return sbus_msg; +} + +} // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_nodelet.cpp b/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_nodelet.cpp new file mode 100644 index 00000000..6ed39960 --- /dev/null +++ b/interfaces/kr_sbus_interface/src/so3cmd_to_sbus_nodelet.cpp @@ -0,0 +1,197 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +class SO3CmdToSbus : public nodelet::Nodelet +{ + public: + void onInit(void); + + private: + void so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg); + void odom_callback(const nav_msgs::Odometry::ConstPtr &odom); + void imu_callback(const sensor_msgs::Imu::ConstPtr &pose); + void so3_cmd_to_sbus_interface(const kr_mav_msgs::SO3Command::ConstPtr &msg); + void motors_on(); + void motors_off(); + + // controller state + bool odom_set_, imu_set_, so3_cmd_set_; + Eigen::Quaterniond odom_q_, imu_q_; + + ros::Subscriber so3_cmd_sub_; + ros::Subscriber odom_sub_; + ros::Subscriber imu_sub_; + + int motor_status_; + + double so3_cmd_timeout_; + ros::Time last_so3_cmd_time_; + kr_mav_msgs::SO3Command last_so3_cmd_; + + sbus_bridge::SBusBridge sbus_bridge_; +}; + +void SO3CmdToSbus::odom_callback(const nav_msgs::Odometry::ConstPtr &odom) +{ + if(!odom_set_) + odom_set_ = true; + + odom_q_ = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, + odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); + + if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) + { + ROS_DEBUG("so3_cmd timeout. %f seconds since last command", (ros::Time::now() - last_so3_cmd_time_).toSec()); + const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); + + so3_cmd_callback(last_so3_cmd_ptr); + } +} + +void SO3CmdToSbus::imu_callback(const sensor_msgs::Imu::ConstPtr &pose) +{ + if(!imu_set_) + imu_set_ = true; + + imu_q_ = Eigen::Quaterniond(pose->orientation.w, pose->orientation.x, pose->orientation.y, pose->orientation.z); + + if(so3_cmd_set_ && ((ros::Time::now() - last_so3_cmd_time_).toSec() >= so3_cmd_timeout_)) + { + ROS_DEBUG("so3_cmd timeout. %f seconds since last command", (ros::Time::now() - last_so3_cmd_time_).toSec()); + const auto last_so3_cmd_ptr = boost::make_shared(last_so3_cmd_); + + so3_cmd_callback(last_so3_cmd_ptr); + } +} + +void SO3CmdToSbus::motors_on() +{ + // call the update 0 success +// int res_update = sn_update_data(); +// if(res_update == -1) +// { +// ROS_ERROR("Likely failure in snav, ensure it is running"); +// return; +// } + +// switch(snav_cached_data_struct_->general_status.props_state) +// { +// case SN_PROPS_STATE_NOT_SPINNING: +// { +// sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); +// int ret = sn_spin_props(); +// if(ret == -1) +// ROS_ERROR("Not able to send spinning command"); +// break; +// } +// case SN_PROPS_STATE_STARTING: +// { +// ROS_WARN("Propellers are starting to spin"); +// break; +// } +// case SN_PROPS_STATE_SPINNING: +// { +// ROS_INFO("Propellers are spinning"); +// motor_status_ = 1; +// break; +// } +// default: +// ROS_ERROR("SN_PROPS_STATE_UNKNOWN"); +// } +} + +void SO3CmdToSbus::motors_off() +{ +// do +// { +// // call the update, 0-success +// int res_update = sn_update_data(); +// if(res_update == -1) +// { +// ROS_ERROR("Likely failure in snav, ensure it is running"); +// } + +// // send minimum thrust and identity attitude +// sn_send_thrust_att_ang_vel_command(0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); + +// // stop the props, 0-success +// int r = sn_stop_props(); +// if(r == 0) +// motor_status_ = 0; +// else if(r == -1) +// { +// ROS_ERROR("Not able to send switch off propellers"); +// } +// } while(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING); + +// // check the propellers status +// if(snav_cached_data_struct_->general_status.props_state == SN_PROPS_STATE_SPINNING) +// { +// ROS_ERROR("All the propellers are still spinning"); +// motor_status_ = 1; +// } +} + +void SO3CmdToSbus::so3_cmd_to_sbus_interface(const kr_mav_msgs::SO3Command::ConstPtr &msg) +{ + sbus_bridge_.controlCommandCallback(msg, odom_q_); +} + +void SO3CmdToSbus::so3_cmd_callback(const kr_mav_msgs::SO3Command::ConstPtr &msg) +{ + if(!so3_cmd_set_) + so3_cmd_set_ = true; + + // switch on motors + if(msg->aux.enable_motors && !motor_status_) + motors_on(); + else if(!msg->aux.enable_motors) + motors_off(); + + so3_cmd_to_sbus_interface(msg); + + // save last so3_cmd + last_so3_cmd_ = *msg; + last_so3_cmd_time_ = ros::Time::now(); +} + +void SO3CmdToSbus::onInit(void) +{ + ros::NodeHandle priv_nh(getPrivateNodeHandle()); + + // get param for so3 command timeout duration + priv_nh.param("so3_cmd_timeout", so3_cmd_timeout_, 0.25); + + odom_set_ = false; + imu_set_ = false; + so3_cmd_set_ = false; + motor_status_ = 0; +// snav_cached_data_struct_ = NULL; + +// if(sn_get_flight_data_ptr(sizeof(SnavCachedData), &snav_cached_data_struct_) != 0) +// { +// ROS_ERROR("Failed to get flight data pointer!"); +// return; +// } + // attitude_raw_pub_ = + // priv_nh.advertise("attitude_raw", 10); + + so3_cmd_sub_ = + priv_nh.subscribe("so3_cmd", 10, &SO3CmdToSbus::so3_cmd_callback, this, ros::TransportHints().tcpNoDelay()); + + odom_sub_ = priv_nh.subscribe("odom", 10, &SO3CmdToSbus::odom_callback, this, ros::TransportHints().tcpNoDelay()); + + imu_sub_ = priv_nh.subscribe("imu", 10, &SO3CmdToSbus::imu_callback, this, ros::TransportHints().tcpNoDelay()); +} + +#include +PLUGINLIB_EXPORT_CLASS(SO3CmdToSbus, nodelet::Nodelet); diff --git a/interfaces/kr_sbus_interface/src/thrust_mapping.cpp b/interfaces/kr_sbus_interface/src/thrust_mapping.cpp new file mode 100644 index 00000000..077947af --- /dev/null +++ b/interfaces/kr_sbus_interface/src/thrust_mapping.cpp @@ -0,0 +1,76 @@ +#include "kr_sbus_interface/thrust_mapping.h" + +#include + +namespace thrust_mapping { + +CollectiveThrustMapping::CollectiveThrustMapping() + : thrust_map_a_(0.0), + thrust_map_b_(0.0), + thrust_map_c_(0.0), + perform_thrust_voltage_compensation_(false), + thrust_ratio_voltage_map_a_(0.0), + thrust_ratio_voltage_map_b_(0.0), + n_lipo_cells_(0) {} + +CollectiveThrustMapping::CollectiveThrustMapping( + const double thrust_map_a, const double thrust_map_b, + const double thrust_map_c, const bool perform_thrust_voltage_compensation, + const double thrust_ratio_voltage_map_a, + const double thrust_ratio_voltage_map_b, const int n_lipo_cells) + : thrust_map_a_(thrust_map_a), + thrust_map_b_(thrust_map_b), + thrust_map_c_(thrust_map_c), + perform_thrust_voltage_compensation_(perform_thrust_voltage_compensation), + thrust_ratio_voltage_map_a_(thrust_ratio_voltage_map_a), + thrust_ratio_voltage_map_b_(thrust_ratio_voltage_map_b), + n_lipo_cells_(n_lipo_cells) {} + +CollectiveThrustMapping::~CollectiveThrustMapping() {} + +uint16_t CollectiveThrustMapping::inverseThrustMapping( + const double thrust, const double battery_voltage) const { + double thrust_applied = thrust; + if (perform_thrust_voltage_compensation_) { + if (battery_voltage >= + n_lipo_cells_ * kMinBatteryCompensationVoltagePerCell_ && + battery_voltage <= + n_lipo_cells_ * kMaxBatteryCompensationVoltagePerCell_) { + const double thrust_cmd_voltage_ratio = + thrust_ratio_voltage_map_a_ * battery_voltage + + thrust_ratio_voltage_map_b_; + thrust_applied *= thrust_cmd_voltage_ratio; + } else { + ROS_WARN_THROTTLE(1.0, + "[%s] Battery voltage out of range for compensation", + ros::this_node::getName().c_str()); + } + } + + //Citardauq Formula: Gives a numerically stable solution of the quadratic equation for thrust_map_a ~ 0, which is not the case for the standard formula. + const uint16_t cmd = 2.0 * (thrust_map_c_ - thrust_applied) / (-thrust_map_b_ - sqrt(thrust_map_b_ * thrust_map_b_ - 4.0 * thrust_map_a_ * (thrust_map_c_ - thrust_applied))); + + return cmd; +} + +// bool CollectiveThrustMapping::loadParameters() { +// ros::NodeHandle pnh("~"); + +// #define GET_PARAM(name) \ +// if (!quadrotor_common::getParam(#name, name##_, pnh)) return false + +// GET_PARAM(thrust_map_a); +// GET_PARAM(thrust_map_b); +// GET_PARAM(thrust_map_c); + +// GET_PARAM(perform_thrust_voltage_compensation); +// GET_PARAM(thrust_ratio_voltage_map_a); +// GET_PARAM(thrust_ratio_voltage_map_b); +// GET_PARAM(n_lipo_cells); + +// return true; + +// #undef GET_PARAM +// } + +} // namespace thrust_mapping