From a2555e17ef41f0f984c17ac912e6bbdebc655ce3 Mon Sep 17 00:00:00 2001
From: vivek-shankar <robovivek@gmail.com>
Date: Sun, 19 Feb 2017 21:04:07 -0500
Subject: [PATCH] sysid and fixes for update

---
 launch/rosbuzz_2_parallel.launch |   1 +
 src/vec2.bzz                     | 107 +++++++++++++++++++++++++++++++
 2 files changed, 108 insertions(+)
 create mode 100644 src/vec2.bzz

diff --git a/launch/rosbuzz_2_parallel.launch b/launch/rosbuzz_2_parallel.launch
index 44841ea..de18b18 100644
--- a/launch/rosbuzz_2_parallel.launch
+++ b/launch/rosbuzz_2_parallel.launch
@@ -3,6 +3,7 @@
 <launch>
 		<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<param name="bzzfile_name" value="/home/vivek/catkin_ws/src/rosbuzz/src/test1.bzz" />
+		<rosparam file="/home/vivek/catkin_ws/src/rosbuzz/launch/launch_config/m100.yaml"/>
                 <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="rc_cmd" />
 		<param name="fcclient_name" value="/djicmd" />
diff --git a/src/vec2.bzz b/src/vec2.bzz
new file mode 100644
index 0000000..e2fb9b0
--- /dev/null
+++ b/src/vec2.bzz
@@ -0,0 +1,107 @@
+#
+# 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
+}