Added mavlink telemtry

interfaces/kr_sbus_interface/parameters/default.yaml

@@ -4,7 +4,7 @@ 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
+disable_thrust_mapping: true
 # 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

interfaces/kr_sbus_interface/parameters/race_quad.yaml

@@ -4,7 +4,7 @@ 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
+disable_thrust_mapping: true
 # 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

interfaces/kr_sbus_interface/scripts/mavlink_telemetry.py

@@ -0,0 +1,35 @@
+import rospy
+from pymavlink import mavutil
+from sensor_msgs.msg import BatteryState
+
+def mavlink_connect(port='/dev/ttyUSB0', baudrate=115200):
+    """Connect to a MAVLink source"""
+    return mavutil.mavlink_connection(port, baud=baudrate)
+
+def battery_status_publisher():
+    rospy.init_node('battery_status_publisher', anonymous=True)
+    battery_pub = rospy.Publisher('battery_status', BatteryState, queue_size=10)
+    rate = rospy.Rate(100)
+
+    # Connect to MAVLink (adjust port and baudrate as necessary)
+    mav = mavlink_connect(port='/dev/ttyUSB0', baudrate=115200)
+
+    while not rospy.is_shutdown():
+        message = mav.recv_match(blocking=True)
+        if message:
+            if message.get_type() == "SYS_STATUS":
+                battery_msg = BatteryState()
+                battery_msg.voltage = message.voltage_battery / 1000.0 / 4.0
+                battery_msg.current = message.current_battery / 100.0
+                battery_msg.percentage = message.battery_remaining
+
+                battery_msg.header.stamp = rospy.Time.now()
+                battery_pub.publish(battery_msg)
+
+        rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        battery_status_publisher()
+    except rospy.ROSInterruptException:
+        pass

interfaces/kr_sbus_interface/src/sbus_serial_port.cpp

@@ -48,7 +48,6 @@ bool SBusSerialPort::connectSerialPort(const std::string& 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",
@@ -63,6 +62,9 @@ bool SBusSerialPort::connectSerialPort(const std::string& port) {
     return false;
   }
 
+  ROS_INFO("[%s] Connected to serial port %s",
+           ros::this_node::getName().c_str(), port.c_str());
+
   return true;
 }
 

interfaces/kr_sbus_interface/src/so3cmd_to_sbus_nodelet.cpp

@@ -75,6 +75,7 @@ void SO3CmdToSbus::imu_callback(const sensor_msgs::Imu::ConstPtr &pose)
 
 void SO3CmdToSbus::motors_on()
 {
+  ROS_INFO("Motors on");
   sbus_bridge_.armBridge();
   motor_status_ = 1;
   // call the update 0 success