Merge pull request #1148 from Kiwa21/pr-param-value

param plugin : add msg and publisher to catch latest param value
mavlink · Jan 3, 2019 · bdc169a · bdc169a
2 parents 21e0ad6 + 46ce2c6
commit bdc169a
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 0 deletions.
diff --git a/mavros/src/plugins/param.cpp b/mavros/src/plugins/param.cpp
@@ -22,6 +22,7 @@
 #include <mavros_msgs/ParamGet.h>
 #include <mavros_msgs/ParamPull.h>
 #include <mavros_msgs/ParamPush.h>
+#include <mavros_msgs/Param.h>
 
 namespace mavros {
 namespace std_plugins {
@@ -356,6 +357,8 @@ class ParamPlugin : public plugin::PluginBase {
 		set_srv = param_nh.advertiseService("set", &ParamPlugin::set_cb, this);
 		get_srv = param_nh.advertiseService("get", &ParamPlugin::get_cb, this);
 
+		param_value_pub = param_nh.advertise<mavros_msgs::Param>("param_value", 10);
+
 		shedule_timer = param_nh.createTimer(BOOTUP_TIME_DT, &ParamPlugin::shedule_cb, this, true);
 		shedule_timer.stop();
 		timeout_timer = param_nh.createTimer(PARAM_TIMEOUT_DT, &ParamPlugin::timeout_cb, this, true);
@@ -382,6 +385,8 @@ class ParamPlugin : public plugin::PluginBase {
 	ros::ServiceServer set_srv;
 	ros::ServiceServer get_srv;
 
+	ros::Publisher param_value_pub;
+
 	ros::Timer shedule_timer;			//!< for startup shedule fetch
 	ros::Timer timeout_timer;			//!< for timeout resend
 
@@ -420,6 +425,13 @@ class ParamPlugin : public plugin::PluginBase {
 
 		auto param_id = mavlink::to_string(pmsg.param_id);
 
+		mavros_msgs::Param param_msg;
+		param_msg.param_id =  param_id;
+        param_msg.param_type = pmsg.param_type;
+        param_msg.param_count = pmsg.param_count;
+		param_msg.param_index = pmsg.param_index;
+	 	param_value_pub.publish(param_msg)
+
 		// search
 		auto param_it = parameters.find(param_id);
 		if (param_it != parameters.end()) {

diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt
@@ -32,6 +32,7 @@ add_message_files(
   Mavlink.msg
   OpticalFlowRad.msg
   OverrideRCIn.msg
+  Param.msg
   ParamValue.msg
   PositionTarget.msg
   RCIn.msg

diff --git a/mavros_msgs/msg/Param.msg b/mavros_msgs/msg/Param.msg
@@ -0,0 +1,7 @@
+# Parameter msg.
+
+string param_id
+mavros_msgs/ParamValue value
+uint8 param_type
+uint16 param_count
+uint16 param_index