fix m100 launch

launch/launch_config/m100.yaml

@@ -1,5 +1,6 @@
 topics:
   gps      :   global_position
+  localpos :	local_position
   battery  :   power_status
   status   :   flight_status
   fcclient :   dji_mavcmd

script/string.bzz

@@ -0,0 +1,92 @@
+#
+# Returns the string character at the given position.
+# PARAM s: The string
+# PARAM n: The position of the wanted character
+# RETURN The character at the wanted position, or nil
+#
+string.charat = function(s, n) {
+  return string.sub(s, n, n+1)
+}
+
+#
+# Returns the index of the first occurrence of the given string m
+# within another string s. If none is found, this function returns
+# nil.
+# PARAM s: The string
+# PARAM m: The string to match
+# RETURN: The position of the first match, or nil
+#
+string.indexoffirst = function(s, m) {
+  var ls = string.length(s)
+  var lm = string.length(m)
+  var i = 0
+  while(i < ls-lm+1) {
+    if(string.sub(s, i, i+lm) == m) return i
+    i = i + 1
+  }
+  return nil
+}
+
+#
+# Returns the index of the last occurrence of the given string m
+# within another string s. If none is found, this function returns
+# nil.
+# PARAM s: The string
+# PARAM m: The string to match
+# RETURN: The position of the last match, or nil
+#
+string.indexoflast = function(s, m) {
+  var ls = string.length(s)
+  var lm = string.length(m)
+  var i = ls - lm + 1
+  while(i >= 0) {
+    if(string.sub(s, i, i+lm) == m) return i
+    i = i - 1
+  }
+  return nil
+}
+
+# Splits a string s using the delimiters in d. The string list is
+# returned in a table indexed by value (starting at 0).
+# PARAM s: The string
+# PARAM d: A string containing the delimiters
+# RETURN: A table containing the tokens
+string.split = function(s, d) {
+  var i1 = 0  # index to move along s (token start)
+  var i2 = 0  # index to move along s (token end)
+  var c = 0   # token count
+  var t = {}  # token list
+  var ls = string.length(s)
+  var ld = string.length(d)
+  # Go through string s
+  while(i2 < ls) {
+    # Try every delimiter
+    var j = 0   # index to move along d
+    var f = nil # whether the delimiter was found or not
+    while(j < ld and (not f)) {
+      if(string.charat(s, i2) == string.charat(d, j)) {
+        # Delimiter found
+        f = 1
+        # Is it worth adding a new token?
+        if(i2 > i1) {
+          t[c] = string.sub(s, i1, i2)
+          c = c + 1
+        }
+        # Start new token
+        i1 = i2 + 1
+      }
+      else {
+        # Next delimiter
+        j = j + 1
+      }
+    }
+    # Next string character
+    i2 = i2 + 1
+  }
+  # Is it worth adding a new token?
+  if(i2 > i1) {
+    t[c] = string.sub(s, i1, i2)
+  }
+  # Return token list
+  return t;
+}

script/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
+}

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
+}