From 2b65b0fc48fddbe19dc5ae7908a5bcabf55b9502 Mon Sep 17 00:00:00 2001 From: Dexter Date: Wed, 28 Feb 2024 01:51:59 +0000 Subject: [PATCH] Working rc override --- .../kr_sbus_interface/launch/test.launch | 3 + .../kr_sbus_interface/parameters/default.yaml | 26 +++ .../parameters/race_quad.yaml | 26 +++ .../kr_sbus_interface/src/sbus_bridge.cpp | 206 ++++++++++-------- .../kr_sbus_interface/src/thrust_mapping.cpp | 89 ++++---- 5 files changed, 222 insertions(+), 128 deletions(-) create mode 100644 interfaces/kr_sbus_interface/parameters/default.yaml create mode 100644 interfaces/kr_sbus_interface/parameters/race_quad.yaml diff --git a/interfaces/kr_sbus_interface/launch/test.launch b/interfaces/kr_sbus_interface/launch/test.launch index ab1dae90..bc6b46a5 100644 --- a/interfaces/kr_sbus_interface/launch/test.launch +++ b/interfaces/kr_sbus_interface/launch/test.launch @@ -13,6 +13,9 @@ output="screen"> + + + diff --git a/interfaces/kr_sbus_interface/parameters/default.yaml b/interfaces/kr_sbus_interface/parameters/default.yaml new file mode 100644 index 00000000..22eb1827 --- /dev/null +++ b/interfaces/kr_sbus_interface/parameters/default.yaml @@ -0,0 +1,26 @@ +port_name: /dev/ttyUSB0 +enable_receiving_sbus_messages: true +control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout' +# set in the 'flight_controller'!) +rc_timeout: 0.1 # [s] +mass: 0.865 # [kg] +disable_thrust_mapping: false +# Note: When updating the thrust mapping also the voltage dependency mapping +# has to be updated! +# thrust = thrust_map_a * u^2 + thrust_map_b * u + thrust_map_c +thrust_map_a: 6.91194111204e-06 # +thrust_map_b: 0.00754094874204 # +thrust_map_c: -6.01740637316 # +# Maximum values for body rates and roll and pitch angles as they are set +# on the Flight Controller. The max roll an pitch angles are only active +# when flying in angle mode +max_roll_rate: 804.0 # [deg/s] +max_pitch_rate: 804.0 # [deg/s] +max_yaw_rate: 400.0 # [deg/s] +max_roll_angle: 50 # [deg] +max_pitch_angle: 50 # [deg] +alpha_vbat_filter: 0.01 +perform_thrust_voltage_compensation: false +thrust_ratio_voltage_map_a: -0.17044342 # [1/V] +thrust_ratio_voltage_map_b: 3.10495276 # [-] +n_lipo_cells: 3 # [-] diff --git a/interfaces/kr_sbus_interface/parameters/race_quad.yaml b/interfaces/kr_sbus_interface/parameters/race_quad.yaml new file mode 100644 index 00000000..07db4ea3 --- /dev/null +++ b/interfaces/kr_sbus_interface/parameters/race_quad.yaml @@ -0,0 +1,26 @@ +port_name: /dev/ttySAC0 +enable_receiving_sbus_messages: false +control_command_timeout: 0.5 # [s] (Must be larger than 'state_estimate_timeout' +# set in the 'flight_controller'!) +rc_timeout: 0.1 # [s] +mass: 0.596 # [kg] +disable_thrust_mapping: false +# Note: When updating the thrust mapping also the voltage dependency mapping +# has to be updated! +# thrust = thrust_map_a * u^2 + thrust_map_b * u + thrust_map_c +thrust_map_a: 7.13455275548e-06 +thrust_map_b: 0.0074241470842 +thrust_map_c: -6.03496490208 +# Maximum values for body rates and roll and pitch angles as they are set +# on the Flight Controller. The max roll an pitch angles are only active +# when flying in angle mode +max_roll_rate: 1022.0 # [deg/s] +max_pitch_rate: 1022.0 # [deg/s] +max_yaw_rate: 524.0 # [deg/s] +max_roll_angle: 50 # [deg] +max_pitch_angle: 50 # [deg] +alpha_vbat_filter: 0.01 +perform_thrust_voltage_compensation: true +thrust_ratio_voltage_map_a: -0.17220303 # [1/V] +thrust_ratio_voltage_map_b: 3.13990035 # [-] +n_lipo_cells: 3 # [-] \ No newline at end of file diff --git a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp index 70415253..8efae4f5 100644 --- a/interfaces/kr_sbus_interface/src/sbus_bridge.cpp +++ b/interfaces/kr_sbus_interface/src/sbus_bridge.cpp @@ -21,11 +21,11 @@ SBusBridge::SBusBridge(const ros::NodeHandle& nh, const ros::NodeHandle& pnh) 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 (!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()); @@ -166,53 +166,53 @@ void SBusBridge::watchdogThread() { void SBusBridge::handleReceivedSbusMessage(const SBusMsg& received_sbus_msg) { -// { -// std::lock_guard main_lock(main_mutex_); + { + 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; -// } + time_last_rc_msg_received_ = ros::Time::now(); -// // 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; -// } + 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; + } -// // Main mutex is unlocked here because it goes out of scope -// } + // 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()); } @@ -274,7 +274,6 @@ void SBusBridge::controlCommandCallback( void SBusBridge::sendSBusMessageToSerialPort(const SBusMsg& sbus_msg) { - std::cout << "sent" << std::endl; SBusMsg sbus_message_to_send = sbus_msg; switch (bridge_state_) { @@ -474,43 +473,74 @@ void SBusBridge::publishLowLevelFeedback(const ros::TimerEvent& time) const { // 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 -// } +bool SBusBridge::loadParameters() +{ + if (!pnh_.getParam("port_name", port_name_)) { + ROS_ERROR("[%s] Could not load port_name parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("enable_receiving_sbus_messages", enable_receiving_sbus_messages_)) { + ROS_ERROR("[%s] Could not load enable_receiving_sbus_messages parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("control_command_timeout", control_command_timeout_)) { + ROS_ERROR("[%s] Could not load control_command_timeout parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("rc_timeout", rc_timeout_)) { + ROS_ERROR("[%s] Could not load rc_timeout parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("mass", mass_)) { + ROS_ERROR("[%s] Could not load mass parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("disable_thrust_mapping", disable_thrust_mapping_)) { + ROS_ERROR("[%s] Could not load disable_thrust_mapping parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("max_roll_rate", max_roll_rate_)) { + ROS_ERROR("[%s] Could not load max_roll_rate parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("max_pitch_rate", max_pitch_rate_)) { + ROS_ERROR("[%s] Could not load max_pitch_rate parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("max_yaw_rate", max_yaw_rate_)) { + ROS_ERROR("[%s] Could not load max_yaw_rate parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("max_roll_angle", max_roll_angle_)) { + ROS_ERROR("[%s] Could not load max_roll_angle parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("max_pitch_angle", max_pitch_angle_)) { + ROS_ERROR("[%s] Could not load max_pitch_angle parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("alpha_vbat_filter", alpha_vbat_filter_)) { + ROS_ERROR("[%s] Could not load alpha_vbat_filter parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_)) { + ROS_ERROR("[%s] Could not load perform_thrust_voltage_compensation parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!pnh_.getParam("n_lipo_cells", n_lipo_cells_)) { + ROS_ERROR("[%s] Could not load n_lipo_cells parameter.", pnh_.getNamespace().c_str()); + return false; + } + if (!thrust_mapping_.loadParameters()) { + return false; + } + max_roll_rate_ /= (180.0 / M_PI); + max_pitch_rate_ /= (180.0 / M_PI); + max_yaw_rate_ /= (180.0 / M_PI); + max_roll_angle_ /= (180.0 / M_PI); + max_pitch_angle_ /= (180.0 / M_PI); + + return true; +} } // namespace sbus_bridge diff --git a/interfaces/kr_sbus_interface/src/thrust_mapping.cpp b/interfaces/kr_sbus_interface/src/thrust_mapping.cpp index 077947af..e0b8a2a8 100644 --- a/interfaces/kr_sbus_interface/src/thrust_mapping.cpp +++ b/interfaces/kr_sbus_interface/src/thrust_mapping.cpp @@ -2,7 +2,8 @@ #include -namespace thrust_mapping { +namespace thrust_mapping +{ CollectiveThrustMapping::CollectiveThrustMapping() : thrust_map_a_(0.0), @@ -11,66 +12,74 @@ CollectiveThrustMapping::CollectiveThrustMapping() perform_thrust_voltage_compensation_(false), thrust_ratio_voltage_map_a_(0.0), thrust_ratio_voltage_map_b_(0.0), - n_lipo_cells_(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) +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) {} + n_lipo_cells_(n_lipo_cells) +{ +} CollectiveThrustMapping::~CollectiveThrustMapping() {} -uint16_t CollectiveThrustMapping::inverseThrustMapping( - const double thrust, const double battery_voltage) const { +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_) { + 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_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()); + } + 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))); + // 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 +bool CollectiveThrustMapping::loadParameters() +{ + ros::NodeHandle pnh("~"); -// GET_PARAM(thrust_map_a); -// GET_PARAM(thrust_map_b); -// GET_PARAM(thrust_map_c); + if(!pnh.getParam("thrust_map_a", thrust_map_a_)) + return false; + if(!pnh.getParam("thrust_map_b", thrust_map_b_)) + return false; + if(!pnh.getParam("thrust_map_c", thrust_map_c_)) + return false; + if(!pnh.getParam("perform_thrust_voltage_compensation", perform_thrust_voltage_compensation_)) + return false; + if(!pnh.getParam("thrust_ratio_voltage_map_a", thrust_ratio_voltage_map_a_)) + return false; + if(!pnh.getParam("thrust_ratio_voltage_map_b", thrust_ratio_voltage_map_b_)) + return false; + if(!pnh.getParam("n_lipo_cells", n_lipo_cells_)) + return false; -// 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 -// } + return true; +} } // namespace thrust_mapping