added aux command - untested

BYU-AUVSI · May 31, 2017 · 2e8d97b · 2e8d97b
1 parent 2db2724
commit 2e8d97b
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 1 deletion.
diff --git a/rosflight/include/rosflight/mavlink/v1.0 b/rosflight/include/rosflight/mavlink/v1.0
diff --git a/rosflight/include/rosflight/rosflight_io.h b/rosflight/include/rosflight/rosflight_io.h
@@ -59,6 +59,7 @@
 #include <rosflight_msgs/Barometer.h>
 #include <rosflight_msgs/Airspeed.h>
 #include <rosflight_msgs/Command.h>
+#include <rosflight_msgs/AuxCommand.h>
 #include <rosflight_msgs/OutputRaw.h>
 #include <rosflight_msgs/RCRaw.h>
 #include <rosflight_msgs/Status.h>
@@ -112,6 +113,7 @@ class rosflightIO :
 
   // ROS message callbacks
   void commandCallback(rosflight_msgs::Command::ConstPtr msg);
+  void auxCommandCallback(rosflight_msgs::AuxCommand::ConstPtr msg);
 
   // ROS service callbacks
   bool paramGetSrvCallback(rosflight_msgs::ParamGet::Request &req, rosflight_msgs::ParamGet::Response &res);
@@ -142,6 +144,7 @@ class rosflightIO :
   ros::NodeHandle nh_;
 
   ros::Subscriber command_sub_;
+  ros::Subscriber aux_command_sub_;
 
   ros::Publisher unsaved_params_pub_;
   ros::Publisher imu_pub_;

diff --git a/rosflight/src/rosflight_io.cpp b/rosflight/src/rosflight_io.cpp
@@ -51,6 +51,7 @@ rosflightIO::rosflightIO() :
   prev_control_mode_(0)
 {
   command_sub_ = nh_.subscribe("command", 1, &rosflightIO::commandCallback, this);
+  aux_command_sub_ = nh_.subscribe("aux_command", 1, &rosflightIO::auxCommandCallback, this);
 
   unsaved_params_pub_ = nh_.advertise<std_msgs::Bool>("unsaved_params", 1, true);
 
@@ -722,6 +723,20 @@ void rosflightIO::commandCallback(rosflight_msgs::Command::ConstPtr msg)
   mavrosflight_->serial.send_message(mavlink_msg);
 }
 
+void rosflightIO::auxCommandCallback(rosflight_msgs::AuxCommand::ConstPtr msg)
+{
+  uint8_t types[8];
+  float values[8];
+  for (int i = 0; i < 8; i++)
+  {
+    types[i] = msg->type_array[i];
+    values[i] = msg->values[i];
+  }
+  mavlink_message_t mavlink_msg;
+  mavlink_msg_rosflight_aux_cmd_pack(1, 50, &mavlink_msg, types, values);
+  mavrosflight_->serial.send_message(mavlink_msg);
+}
+
 bool rosflightIO::paramGetSrvCallback(rosflight_msgs::ParamGet::Request &req, rosflight_msgs::ParamGet::Response &res)
 {
   res.exists = mavrosflight_->param.get_param_value(req.name, &res.value);

diff --git a/rosflight_msgs/CMakeLists.txt b/rosflight_msgs/CMakeLists.txt
@@ -19,6 +19,7 @@ add_message_files(
   Attitude.msg
   State.msg
   Status.msg
+  AuxCommand.msg
 )
 
 add_service_files(

diff --git a/rosflight_msgs/msg/AuxCommand.msg b/rosflight_msgs/msg/AuxCommand.msg
@@ -0,0 +1,11 @@
+# Auxilliary Override Command Message
+
+# Command Types
+uint8 AUX_COMMAND_DISABLED = 0
+uint8 AUX_COMMAND_SERVO = 1
+uint8 AUX_COMMAND_MOTOR = 2
+
+Header header
+uint8[8] type_array
+float32[8] values
+