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