diff --git a/include/controller_base.h b/include/controller_base.h index 09d8191..3200f56 100644 --- a/include/controller_base.h +++ b/include/controller_base.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -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 index d4c35ee..c37a4cf 100644 --- 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("command",10); + _extended_actuators_pub = nh_.advertise("extended_command", 10); _att_cmd_pub = nh_.advertise("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;