diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index d2ec0879f9..2b13ecd174 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -67,3 +67,43 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou
     }
     return ROTATION_NONE;
 }
+
+// constrain a value
+float constrain(float amt, float low, float high) {
+	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
+}
+
+// constrain a int16_t value
+int16_t constrain_int16(int16_t amt, int16_t low, int16_t high) {
+	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
+}
+
+// constrain a int32_t value
+int32_t constrain_int32(int32_t amt, int32_t low, int32_t high) {
+	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
+}
+
+// degrees -> radians
+float radians(float deg) {
+	return deg * DEG_TO_RAD;
+}
+
+// radians -> degrees
+float degrees(float rad) {
+	return rad * RAD_TO_DEG;
+}
+
+// square
+float sq(float v) {
+	return v*v;
+}
+
+// 2D vector length
+float pythagorous2(float a, float b) {
+	return sqrt(sq(a)+sq(b));
+}
+
+// 3D vector length
+float pythagorous3(float a, float b, float c) {
+	return sqrt(sq(a)+sq(b)+sq(c));
+}
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 300bd81bbd..c4e27c3ad4 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -61,17 +61,32 @@ void        location_update(struct Location *loc, float bearing, float distance)
 // extrapolate latitude/longitude given distances north and east
 void        location_offset(struct Location *loc, float ofs_north, float ofs_east);
 
+// constrain a value
+float   constrain(float amt, float low, float high);
+int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);
+int32_t constrain_int32(int32_t amt, int32_t low, int32_t high);
+
+// degrees -> radians
+float radians(float deg);
+
+// radians -> degrees
+float degrees(float rad);
+
+// square
+float sq(float v);
+
+// sqrt of sum of squares
+float pythagorous2(float a, float b);
+float pythagorous3(float a, float b, float c);
+
 #ifdef radians
 #error "You need to add empty nocore.inoflag and Arduino.h files to your sketch"
 #endif
 
 /* The following three functions used to be arduino core macros */
-#define radians(deg) ((deg) * DEG_TO_RAD)
-#define degrees(rad) ((rad) * RAD_TO_DEG)
-#define sq(x) ((x)*(x))
 #define max(a,b) ((a)>(b)?(a):(b))
 #define min(a,b) ((a)<(b)?(a):(b))
-#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
 
 #endif // AP_MATH_H
 
diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp
index 0671d54f1f..ef33f7fe4b 100644
--- a/libraries/AP_Math/location.cpp
+++ b/libraries/AP_Math/location.cpp
@@ -53,7 +53,7 @@ float get_distance(const struct Location *loc1, const struct Location *loc2)
         return -1;
     float dlat              = (float)(loc2->lat - loc1->lat);
     float dlong             = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2);
-    return sqrt(sq(dlat) + sq(dlong)) * .01113195;
+    return pythagorous2(dlat, dlong);
 }
 
 // return distance in centimeters to between two locations, or -1 if
diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
new file mode 100644
index 0000000000..03c8d9ce67
--- /dev/null
+++ b/libraries/AP_Math/vector2.cpp
@@ -0,0 +1,28 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * vector3.cpp
+ * Copyright (C) Andrew Tridgell 2012
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_Math.h"
+
+template <typename T>
+float Vector2<T>::length(void) const
+{
+	return pythagorous2(x, y);
+}
+
+template float Vector2<float>::length(void) const;
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index c85ffb7bf0..3c4d0317f2 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -130,10 +130,7 @@ struct Vector2
     }
 
     // gets the length of this vector
-    T   length() const
-    {
-        return (T)sqrt(*this * *this);
-    }
+    float           length(void) const;
 
     // normalizes this vector
     void    normalize()
diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp
index 72ea1e961a..cdad957e09 100644
--- a/libraries/AP_Math/vector3.cpp
+++ b/libraries/AP_Math/vector3.cpp
@@ -127,7 +127,7 @@ T Vector3<T>::operator *(const Vector3<T> &v) const
 template <typename T>
 float Vector3<T>::length(void) const
 {
-    return (T)sqrt(*this * *this);
+    return pythagorous3(x, y, z);
 }
 
 // only define for signed numbers