sys_status.cpp: Add a SYS_STATUS message publisher

mavlink · Feb 29, 2024 · 323ce6b · 323ce6b
1 parent 92e4062
commit 323ce6b
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 0 deletions.
diff --git a/mavros/src/plugins/sys_status.cpp b/mavros/src/plugins/sys_status.cpp
@@ -27,6 +27,7 @@
 #include <mavros_msgs/VehicleInfo.h>
 #include <mavros_msgs/VehicleInfoGet.h>
 #include <mavros_msgs/MessageInterval.h>
+#include <mavros_msgs/SysStatus.h>
 
 
 #ifdef HAVE_SENSOR_MSGS_BATTERYSTATE_MSG
@@ -556,6 +557,7 @@ class SystemStatusPlugin : public plugin::PluginBase
 		batt_pub = nh.advertise<BatteryMsg>("battery", 10);
 		estimator_status_pub = nh.advertise<mavros_msgs::EstimatorStatus>("estimator_status", 10);
 		statustext_pub = nh.advertise<mavros_msgs::StatusText>("statustext/recv", 10);
+		sys_status_pub = nh.advertise<mavros_msgs::SysStatus>("sys_status", 10);
 		statustext_sub = nh.subscribe("statustext/send", 10, &SystemStatusPlugin::statustext_cb, this);
 		rate_srv = nh.advertiseService("set_stream_rate", &SystemStatusPlugin::set_rate_cb, this);
 		mode_srv = nh.advertiseService("set_mode", &SystemStatusPlugin::set_mode_cb, this);
@@ -598,6 +600,7 @@ class SystemStatusPlugin : public plugin::PluginBase
 	ros::Publisher batt_pub;
 	ros::Publisher estimator_status_pub;
 	ros::Publisher statustext_pub;
+	ros::Publisher sys_status_pub;
 	ros::Subscriber statustext_sub;
 	ros::ServiceServer rate_srv;
 	ros::ServiceServer mode_srv;
@@ -832,6 +835,25 @@ class SystemStatusPlugin : public plugin::PluginBase
 
 		sys_diag.set(stat);
 
+		mavros_msgs::SysStatus sys_status;
+		sys_status.header.stamp = ros::Time::now();
+		sys_status.sensors_present = stat.onboard_control_sensors_present;
+		sys_status.sensors_enabled = stat.onboard_control_sensors_enabled;
+		sys_status.sensors_health = stat.onboard_control_sensors_health;
+
+		sys_status.load = stat.load;
+		sys_status.voltage_battery = stat.voltage_battery;
+		sys_status.current_battery = stat.current_battery;
+		sys_status.battery_remaining = stat.battery_remaining;
+		sys_status.drop_rate_comm = stat.drop_rate_comm;
+		sys_status.errors_comm = stat.errors_comm;
+		sys_status.errors_count1 = stat.errors_count1;
+		sys_status.errors_count2 = stat.errors_count2;
+		sys_status.errors_count3 = stat.errors_count3;
+		sys_status.errors_count4 = stat.errors_count4;
+
+		sys_status_pub.publish(sys_status);
+
 		if (has_battery_status0)
 			return;
 

diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt
@@ -60,6 +60,7 @@ add_message_files(
   RTKBaseline.msg
   State.msg
   StatusText.msg
+  SysStatus.msg
   TerrainReport.msg
   Thrust.msg
   TimesyncStatus.msg

diff --git a/mavros_msgs/msg/SysStatus.msg b/mavros_msgs/msg/SysStatus.msg
@@ -0,0 +1,15 @@
+std_msgs/Header header
+
+uint32 sensors_present
+uint32 sensors_enabled
+uint32 sensors_health
+uint16 load
+uint16 voltage_battery
+int16 current_battery
+int8 battery_remaining
+uint16 drop_rate_comm
+uint16 errors_comm
+uint16 errors_count1
+uint16 errors_count2
+uint16 errors_count3
+uint16 errors_count4