fix for m100

launch/launch_config/m100.yaml

@@ -2,6 +2,7 @@ topics:
   gps      :   /global_position
   battery  :   /power_status
   status   :   /flight_status
+  fcclient :   /dji_mavcmd
 type:
   gps  :  sensor_msgs/NavSatFix
   battery :   mavros_msgs/BatteryStatus

launch/launch_config/solo.yaml

@@ -2,6 +2,7 @@ topics:
   gps     :   /mavros/global_position/global
   battery :   /mavros/battery
   status  :   /mavros/state
+  fcclient:   /mavros/cmd/command
 type:
   gps  :  sensor_msgs/NavSatFix
   # for SITL Solo 

launch/rosbuzz.launch

@@ -6,8 +6,6 @@
 		<param name="bzzfile_name" value="$(env ROS_WS)/src/ROSBuzz/src/test1.bzz" />
         <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
-		<!-- todo: verify if this will work, or use "command" -->
-		<param name="fcclient_name" value="/mavros/cmd/command" />
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="robot_id" value="3"/>

src/roscontroller.cpp

@@ -125,9 +125,6 @@ namespace rosbzz_node{
      			else if(rcclient==false){ROS_INFO("RC service is disabled");}
   		}
   		else{ROS_ERROR("Provide a rc client option: yes or no in Launch file"); system("rosnode kill rosbuzz_node");} 
-  		/*Obtain fc client name from parameter server*/
-  		if(n_c.getParam("fcclient_name", fcclient_name));
-  		else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}  
   		/*Obtain robot_id from parameter server*/
   		n_c.getParam("robot_id", robot_id);
 		/*Obtain out payload name*/
@@ -162,6 +159,10 @@ namespace rosbzz_node{
 		node_handle.getParam("topics/status", status_topic);
 		node_handle.getParam("type/status", status_type);
 		m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, status_type));
+
+  		/*Obtain fc client name from parameter server*/
+  		if(node_handle.getParam("topics/fcclient", fcclient_name));
+  		else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}  
 	}
 
 	void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
@@ -256,8 +257,8 @@ namespace rosbzz_node{
 	    		else ROS_ERROR("Failed to call service from flight controller"); 
 		} else if (tmp == 2) { /*FC call for goto*/ 
 			/*prepare the goto publish message */
-			cmd_srv.request.param5 = goto_pos[0]*pow(10,7);
+			cmd_srv.request.param5 = goto_pos[0];
-    			cmd_srv.request.param6 = goto_pos[1]*pow(10,7);
+    			cmd_srv.request.param6 = goto_pos[1];
     			cmd_srv.request.param7 = goto_pos[2];
     			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
 			if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
@@ -685,11 +686,8 @@ namespace rosbzz_node{
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
    				ROS_INFO("RC_Call: GO TO!!!! ");
 				double rc_goto[3];
-   				//rc_goto[0]=req.x/pow(10,7);
+				rc_goto[0] = req.param5;
-				//rc_goto[1]=req.y/pow(10,7);
+				rc_goto[1] = req.param6;
-				//rc_goto[2]=req.z;
-				rc_goto[0] = req.param5 / pow(10, 7);
-				rc_goto[1] = req.param6 / pow(10, 7);
 				rc_goto[2] = req.param7;
 
 				buzzuav_closures::rc_set_goto(rc_goto);

src/test1.bzz

@@ -1,7 +1,7 @@
 
 # We need this for 2D vectors
 # Make sure you pass the correct include path to "bzzc -I <path1:path2> ..."
-include "/home/ivan/dev/buzz/src/include/vec2.bzz"
+include "/home/ubuntu/buzz/src/include/vec2.bzz"
 ####################################################################################################
 # Updater related
 # This should be here for the updater to work, changing position of code will crash the updater

src/vec2.bzz

@@ -1,107 +0,0 @@
-#
-# Create a new namespace for vector2 functions
-#
-math.vec2 = {}
-
-#
-# Creates a new vector2.
-# PARAM x: The x coordinate.
-# PARAM y: The y coordinate.
-# RETURN: A new vector2.
-#
-math.vec2.new = function(x, y) {
-  return { .x = x, .y = y }
-}
-
-#
-# Creates a new vector2 from polar coordinates.
-# PARAM l: The length of the vector2.
-# PARAM a: The angle of the vector2.
-# RETURN: A new vector2.
-#
-math.vec2.newp = function(l, a) {
-  return {
-    .x = l * math.cos(a),
-    .y = l * math.sin(a)
-  }
-}
-
-#
-# Calculates the length of the given vector2.
-# PARAM v: The vector2.
-# RETURN: The length of the vector.
-#
-math.vec2.length = function(v) {
-  return math.sqrt(v.x * v.x + v.y * v.y)
-}
-
-#
-# Calculates the angle of the given vector2.
-# PARAM v: The vector2.
-# RETURN: The angle of the vector.
-#
-math.vec2.angle = function(v) {
-  return math.atan2(v.y, v.x)
-}
-
-#
-# Returns the normalized form of a vector2.
-# PARAM v: The vector2.
-# RETURN: The normalized form.
-#
-math.vec2.norm = function(v) {
-  var l = math.length(v)
-  return {
-    .x = v.x / l,
-    .y = v.y / l
-  }
-}
-
-#
-# Calculates v1 + v2.
-# PARAM v1: A vector2.
-# PARAM v2: A vector2.
-# RETURN: v1 + v2
-#
-math.vec2.add = function(v1, v2) {
-  return {
-    .x = v1.x + v2.x,
-    .y = v1.y + v2.y
-  }
-}
-
-#
-# Calculates v1 - v2.
-# PARAM v1: A vector2.
-# PARAM v2: A vector2.
-# RETURN: v1 + v2
-#
-math.vec2.sub = function(v1, v2) {
-  return {
-    .x = v1.x - v2.x,
-    .y = v1.y - v2.y
-  }
-}
-
-#
-# Scales a vector by a numeric constant.
-# PARAM v: A vector2.
-# PARAM s: A number (float or int).
-# RETURN: s * v
-#
-math.vec2.scale = function(v, s) {
-  return {
-    .x = v.x * s,
-    .y = v.y * s
-  }
-}
-
-#
-# Calculates v1 . v2 (the dot product)
-# PARAM v1: A vector2.
-# PARAM v2: A vector2.
-# RETURN: v1 . v2
-#
-math.vec2.dot = function(v1, v2) {
-  return v1.x * v2.x + v1.y * v2.y
-}