From 4eb044a4a91058c61738ac675be14ddcd913d6ab Mon Sep 17 00:00:00 2001
From: dave <david.st-onge@polymtl.ca>
Date: Sun, 23 Sep 2018 22:02:05 -0400
Subject: [PATCH] fix modes usage for spiris mavros compatibility

---
 buzz_scripts/include/act/states.bzz |  4 ++--
 include/roscontroller.h             |  1 +
 src/roscontroller.cpp               | 34 ++++++++++++++++++++++-------
 3 files changed, 29 insertions(+), 10 deletions(-)

diff --git a/buzz_scripts/include/act/states.bzz b/buzz_scripts/include/act/states.bzz
index 0869852..0cca81d 100644
--- a/buzz_scripts/include/act/states.bzz
+++ b/buzz_scripts/include/act/states.bzz
@@ -48,7 +48,7 @@ function launch() {
 	BVMSTATE = "LAUNCH"
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so TAKE_OFF
 		homegps = {.lat=pose.position.latitude, .long=pose.position.longitude}
-		if( flight.status == 2 and pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+		if(pose.position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 			barrier_set(ROBOTS, AUTO_LAUNCH_STATE, "STOP", 22)
 			barrier_ready(22)
 		} else {
@@ -77,7 +77,7 @@ function stop() {
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		neighbors.broadcast("cmd", 21)
 		uav_land()
-		if(flight.status != 2 and flight.status != 3) {
+		if(pose.position.altitude <= 0.1) {
 			BVMSTATE = "TURNEDOFF"
 			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
 			#barrier_ready(21)
diff --git a/include/roscontroller.h b/include/roscontroller.h
index d7bbbc5..00a61a3 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -139,6 +139,7 @@ private:
   uint8_t no_cnt = 0;
   uint8_t old_val = 0;
   bool debug = false;
+  bool setmode = false;
   std::string bzzfile_name;
   std::string fcclient_name;
   std::string armclient;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index a16b042..bc4080a 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -30,7 +30,8 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
   buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
   //  Initialize variables
-  SetMode("LOITER", 0);
+  if(setmode)
+    SetMode("LOITER", 0);
   armstate = 0;
   multi_msg = true;
   setpoint_counter = 0;
@@ -251,6 +252,14 @@ void roscontroller::Rosparameters_get(ros::NodeHandle& n_c)
     ROS_ERROR("Provide a debug mode in Launch file");
     system("rosnode kill rosbuzz_node");
   }
+  // Obtain setmode mode from launch file parameter
+  if (n_c.getParam("setmode", setmode))
+    ;
+  else
+  {
+    ROS_ERROR("Provide a setmode mode in Launch file");
+    system("rosnode kill rosbuzz_node");
+  }
   // Obtain rc service option from parameter server
   if (n_c.getParam("rcclient", rcclient))
   {
@@ -403,9 +412,10 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle& n_c)
 
   // Service Clients
   arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
-  mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
+  if(setmode)
+    mode_client = n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
   mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
-  if (rcclient == true)
+  if (rcclient)
     service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
   ROS_INFO("Ready to receive Mav Commands from RC client");
   xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
@@ -719,14 +729,15 @@ script
       cmd_srv.request.command = buzzuav_closures::getcmd();
       if (!armstate)
       {
-        SetMode("LOITER", 0);
+        if(setmode)
+          SetMode("LOITER", 0);
         armstate = 1;
         Arm();
         ros::Duration(0.5).sleep();
         // Registering HOME POINT.
         home = cur_pos;
       }
-      if (current_mode != "GUIDED")
+      if (current_mode != "GUIDED" && setmode)
         SetMode("GUIDED", 2000);  // added for compatibility with 3DR Solo
       if (mav_client.call(cmd_srv))
       {
@@ -738,12 +749,17 @@ script
 
     case NAV_LAND:
       cmd_srv.request.command = buzzuav_closures::getcmd();
-      if (current_mode != "LAND")
+      if (current_mode != "LAND" && setmode)
       {
         SetMode("LAND", 0);
         armstate = 0;
         Arm();
       }
+      else if(cur_pos.altitude < 0.1) //disarm only when close to ground
+      {
+        armstate = 0;
+        Arm();
+      }
       if (mav_client.call(cmd_srv))
       {
         ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
@@ -771,7 +787,8 @@ script
     case COMPONENT_ARM_DISARM:
       if (!armstate)
       {
-        SetMode("LOITER", 0);
+        if(setmode)
+          SetMode("LOITER", 0);
         armstate = 1;
         Arm();
       }
@@ -780,8 +797,9 @@ script
     case COMPONENT_ARM_DISARM+1:
       if (armstate)
       {
+        if(setmode)
+          SetMode("LOITER", 0);
         armstate = 0;
-        SetMode("LOITER", 0);
         Arm();
       }
       break;