autotest: moved constrain() into util.py

Tools/autotest/pysim/gimbal.py

@@ -5,14 +5,6 @@ import util, time, math
 from math import degrees, radians, cos, sin, tan
 from rotmat import Vector3, Matrix3
 
-def constrain(value, minv, maxv):
-    '''constrain a value to a range'''
-    if value < minv:
-        value = minv
-    if value > maxv:
-        value = maxv
-    return value
-
 class Gimbal3Axis(object):
     '''a gimbal simulation'''
     def __init__(self, vehicle):
@@ -154,9 +146,9 @@ class Gimbal3Axis(object):
         #print("gimbalJointRates ", gimbalJointRates)
 
         # 6) Apply the rate limits from 4)
-        gimbalJointRates.x = constrain(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x)
-        gimbalJointRates.y = constrain(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y)
-        gimbalJointRates.z = constrain(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z)
+        gimbalJointRates.x = util.constrain(gimbalJointRates.x, lowerRatelimit.x, upperRatelimit.x)
+        gimbalJointRates.y = util.constrain(gimbalJointRates.y, lowerRatelimit.y, upperRatelimit.y)
+        gimbalJointRates.z = util.constrain(gimbalJointRates.z, lowerRatelimit.z, upperRatelimit.z)
 
         # 7) Convert the modified gimbal joint rates to body rates (still copter
         # relative)

Tools/autotest/pysim/util.py

@@ -413,7 +413,15 @@ def toVec(magnitude, angle):
     m.from_euler(0, 0, angle)
     return m.transposed() * v
 
+def constrain(value, minv, maxv):
+    '''constrain a value to a range'''
+    if value < minv:
+        value = minv
+    if value > maxv:
+        value = maxv
+    return value
 
 if __name__ == "__main__":
     import doctest
     doctest.testmod()
+