Merge branch 'master' of https://github.com/BYU-AUVSI/ros_plane

BYU-AUVSI · Jan 24, 2017 · e8a802e · e8a802e
2 parents 985a76b + 40d6dc7
commit e8a802e
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 0 deletions.
diff --git a/include/controller_base.h b/include/controller_base.h
@@ -13,6 +13,7 @@
 #include <fcu_common/FW_State.h>
 #include <fcu_common/FW_Controller_Commands.h>
 #include <fcu_common/Command.h>
+#include <fcu_common/ExtendedCommand.h>
 #include <fcu_common/FW_Attitude_Commands.h>
 
 #include <dynamic_reconfigure/server.h>
@@ -100,6 +101,7 @@ class controller_base
     ros::Subscriber _vehicle_state_sub;
     ros::Subscriber _controller_commands_sub;
     ros::Publisher _actuators_pub;
+    ros::Publisher _extended_actuators_pub;
     ros::Publisher _att_cmd_pub;
     ros::Timer _act_pub_timer;
 

diff --git a/src/controller_base.cpp b/src/controller_base.cpp
@@ -54,6 +54,7 @@ controller_base::controller_base():
     _server.setCallback(_func);
 
     _actuators_pub = nh_.advertise<fcu_common::Command>("command",10);
+    _extended_actuators_pub = nh_.advertise<fcu_common::ExtendedCommand>("extended_command", 10);
     _att_cmd_pub = nh_.advertise<fcu_common::FW_Attitude_Commands>("attitude_commands",10);
     _act_pub_timer = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base::actuator_controls_publish, this);
 
@@ -148,6 +149,17 @@ void controller_base::actuator_controls_publish(const ros::TimerEvent&)
 
         _actuators_pub.publish(actuators);
 
+        fcu_common::ExtendedCommand extended_actuators;
+        extended_actuators.ignore = 0;
+        extended_actuators.mode = fcu_common::ExtendedCommand::MODE_PASS_THROUGH;
+        extended_actuators.x = output.delta_a;
+        extended_actuators.y = output.delta_e;
+        extended_actuators.z = output.delta_r;
+        extended_actuators.F = output.delta_t;
+
+        _extended_actuators_pub.publish(extended_actuators);
+
+
         if(_att_cmd_pub.getNumSubscribers() > 0)
         {
             fcu_common::FW_Attitude_Commands attitudes;