From ffbe4569cc4045a30c15fb9638e8c45dbcb8f1a1 Mon Sep 17 00:00:00 2001
From: mistTX1 <vivekshankarv@gmail.com>
Date: Tue, 21 Aug 2018 01:40:49 +0000
Subject: [PATCH] mavros version selection as cmake flag

---
 CMakeLists.txt           |  4 ++--
 src/buzzuav_closures.cpp |  8 ++++++++
 src/roscontroller.cpp    | 21 +++++++++++++++++++++
 3 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 672d68d..2b48f35 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,9 +6,9 @@ if(UNIX)
 endif()
 
 if(SIM)
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=1 -DMAVROSKINETIC=1")
 else()
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DSIMULATION=0 -DMAVROSKINETIC=1")
 endif()
 ## Find catkin macros and libraries
 find_package(catkin REQUIRED COMPONENTS
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index e011c61..1e368c5 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -401,7 +401,11 @@ int buzzuav_arm(buzzvm_t vm)
 / Buzz closure to arm
 /---------------------------------------*/
 {
+#ifdef MAVROSKINETIC
+  cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
+#else
   cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
+#endif
   printf(" Buzz requested Arm \n");
   buzz_cmd = COMMAND_ARM;
   return buzzvm_ret0(vm);
@@ -412,7 +416,11 @@ int buzzuav_disarm(buzzvm_t vm)
 / Buzz closure to disarm
 /---------------------------------------*/
 {
+#ifdef MAVROSKINETIC
+  cur_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM + 1;
+#else
   cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
+#endif
   printf(" Buzz requested Disarm  \n");
   buzz_cmd = COMMAND_DISARM;
   return buzzvm_ret0(vm);
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index d2da99a..5b4df3a 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -780,7 +780,11 @@ script
       }
       else
         ROS_ERROR("Failed to call service from flight controller");
+#ifdef MAVROSKINETIC
+      cmd_srv.request.command = mavros_msgs::CommandCode::MISSION_START;
+#else
       cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
+#endif
       if (mav_client.call(cmd_srv))
       {
         ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@@ -818,7 +822,11 @@ script
       cmd_srv.request.param2 = gimbal[1];
       cmd_srv.request.param3 = gimbal[2];
       cmd_srv.request.param4 = gimbal[3];
+#ifdef MAVROSKINETIC
+      cmd_srv.request.command = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
+#else
       cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
+#endif
       if (mav_client.call(cmd_srv))
       {
         ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@@ -1220,8 +1228,13 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
+#ifdef MAVROSKINETIC
+    case mavros_msgs::CommandCode::COMPONENT_ARM_DISARM:
+      rc_cmd = mavros_msgs::CommandCode::COMPONENT_ARM_DISARM;
+#else
     case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
       rc_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
+#endif
       armstate = req.param1;
       if (armstate)
       {
@@ -1249,10 +1262,18 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request& req, mavros_m
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;
+#ifdef MAVROSKINETIC
+    case mavros_msgs::CommandCode::DO_MOUNT_CONTROL:
+#else
     case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
+#endif
       ROS_INFO("RC_Call: Gimbal!!!! ");
       buzzuav_closures::rc_set_gimbal(req.param1, req.param2, req.param3, req.param4, req.param5);
+#ifdef MAVROSKINETIC
+      rc_cmd = mavros_msgs::CommandCode::DO_MOUNT_CONTROL;
+#else
       rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
+#endif
       buzzuav_closures::rc_call(rc_cmd);
       res.success = true;
       break;