add motion API

Signed-off-by: Nethan.lcz <ethan.lcz@alibaba-inc.com>

haas_lib_bundles/python/libraries/motion/detections/fall_detection.py

+ITER_NUM = 50
+ACCELERATION_LOW_THREADHOLD = 4  # 加速度变化下限阈值，越大越灵敏
+ACCELERATION_UP_THREADHOLD = 12  # 加速度变化上限阈值，越小越灵敏
+ANGULAR_VELOCITY_LOW_THREADHOLD = 1  # 角速度变化下限阈值，越小越灵敏
+ANGULAR_VELOCITY_UP_THREADHOLD = 40  # 角速度变化下限阈值，越大越灵敏
+
+
+class fall_detection:
+    def __init__(self, getData):
+        self.ax_offset = 0.0
+        self.ay_offset = 0.0
+        self.az_offset = 0.0
+        self.gx_offset = 0.0
+        self.gy_offset = 0.0
+        self.gz_offset = 0.0
+        self.trigger1count = 0
+        self.trigger2count = 0
+        self.trigger1 = False
+        self.trigger2 = False
+        self.getData = getData
+        self.calibrate()
+
+    def calibrate(self):
+        ax_sum = 0.0
+        ay_sum = 0.0
+        az_sum = 0.0
+        gx_sum = 0.0
+        gy_sum = 0.0
+        gz_sum = 0.0
+        for num in range(0, ITER_NUM):
+            data = self.getData()        # 读取传感器信息值
+            ax_sum += data[0][0]
+            ay_sum += data[0][1]
+            az_sum += data[0][2]
+            gx_sum += data[1][0]
+            gy_sum += data[1][1]
+            gz_sum += data[1][2]
+        self.ax_offset = ax_sum/ITER_NUM
+        self.ay_offset = ay_sum/ITER_NUM
+        self.az_offset = az_sum/ITER_NUM
+        self.gx_offset = gx_sum/ITER_NUM
+        self.gy_offset = gy_sum/ITER_NUM
+        self.gz_offset = gz_sum/ITER_NUM
+        print('Now you can start fall detection!')
+
+    def detect(self):
+        global ACCELERATION_LOW_THREADHOLD
+        global ACCELERATION_UP_THREADHOLD
+        global ANGULAR_VELOCITY_LOW_THREADHOLD
+        global ANGULAR_VELOCITY_UP_THREADHOLD
+        fall = False
+        data = self.getData()                          # 读取传感器信息值
+        AcX = data[0][0]
+        AcY = data[0][1]
+        AcZ = data[0][2]
+        GyX = data[1][0]
+        GyY = data[1][1]
+        GyZ = data[1][2]
+        ax = (AcX-self.ax_offset)
+        ay = (AcY-self.ay_offset)
+        az = (AcZ-self.az_offset)
+        gx = (GyX-self.gx_offset)
+        gy = (GyY-self.gy_offset)
+        gz = (GyZ-self.gz_offset)
+        if hasattr(self, 'sensitivity'):
+            if 'ACCELERATION_LOW_THREADHOLD' in self.sensitivity:
+                ACCELERATION_LOW_THREADHOLD = float(self.sensitivity['ACCELERATION_LOW_THREADHOLD'])
+            if 'ACCELERATION_UP_THREADHOLD' in self.sensitivity:
+                ACCELERATION_UP_THREADHOLD = float(self.sensitivity['ACCELERATION_UP_THREADHOLD'])
+            if 'ANGULAR_VELOCITY_LOW_THREADHOLD' in self.sensitivity:
+                ANGULAR_VELOCITY_LOW_THREADHOLD = float(self.sensitivity['ANGULAR_VELOCITY_LOW_THREADHOLD'])
+            if 'ANGULAR_VELOCITY_UP_THREADHOLD' in self.sensitivity:
+                ANGULAR_VELOCITY_UP_THREADHOLD = float(self.sensitivity['ANGULAR_VELOCITY_UP_THREADHOLD'])
+        # calculating accelerationChangelitute vector for 3 axis
+        Raw_accelerationChange = pow(pow(ax, 2)+pow(ay, 2)+pow(az, 2), 0.5)
+        # Multiplied by 10 to values are between 0 to 1
+        accelerationChange = Raw_accelerationChange * 10
+        # Trigger1 function
+        # if accelerationChange breaks lower threshold
+        if(accelerationChange <= ACCELERATION_LOW_THREADHOLD and self.trigger2 == False):
+            self.trigger1 = True
+            # print("TRIGGER 1 ACTIVATED")
+        if (self.trigger1 == True):
+            self.trigger1count = self.trigger1count + 1
+            # if accelerationChange breaks upper threshold
+            if (accelerationChange >= ACCELERATION_UP_THREADHOLD):
+                self.trigger2 = True
+                # print("TRIGGER 2 ACTIVATED")
+                self.trigger1 = False
+                self.trigger1count = 0
+        if (self.trigger2 == True):
+            self.trigger2count = self.trigger2count+1
+            angleChange = pow(pow(gx, 2)+pow(gy, 2)+pow(gz, 2), 0.5)
+            # Trigger2 function
+            # //if orientation changes by between 1-40 degrees
+            if (angleChange >= ANGULAR_VELOCITY_LOW_THREADHOLD and angleChange <= ANGULAR_VELOCITY_UP_THREADHOLD):
+                self.trigger2 = False
+                self.trigger2count = 0
+                fall = True
+                return fall
+        if (fall == True):  # //in event of a fall detection
+            fall = False
+        if (self.trigger2count >= 50):  # //allow time for orientation change
+            self.trigger2 = False
+            self.trigger2count = 0
+            # print("TRIGGER 2 DECACTIVATED")
+        if (self.trigger1count >= 5):  # //allow time for accelerationChange to break upper threshold
+            self.trigger1 = False
+            self.trigger1count = 0
+            # print("TRIGGER 1 DECACTIVATED")
+        return fall

haas_lib_bundles/python/libraries/motion/detections/tap_detection.py

+import utime   # 延时函数在utime
+
+ITER_NUM = 100
+
+
+class tap_detection:
+    def __init__(self, tap_detect_count, getData):
+        self.ax_offset = 0.0
+        self.ay_offset = 0.0
+        self.az_offset = 0.0
+        self.triggercount = 0
+        self.untriggercount = 0
+        self.tapcount = 0
+        self.tap_detect_count = tap_detect_count
+        if (self.tap_detect_count == 2):
+            self.accelerator_up_threshold = 30.0
+        elif (self.tap_detect_count == 1):
+            self.accelerator_up_threshold = 30.0
+        self.latency = 150
+        self.getData = getData
+        self.calibrate()
+
+    def calibrate(self):
+        ax_sum = 0.0
+        ay_sum = 0.0
+        az_sum = 0.0
+        for num in range(0, ITER_NUM):
+            data = self.getData()        # 读取传感器信息值
+            ax_sum += data[0][0]
+            ay_sum += data[0][1]
+            az_sum += data[0][2]
+        self.ax_offset = ax_sum/ITER_NUM
+        self.ay_offset = ay_sum/ITER_NUM
+        self.az_offset = az_sum/ITER_NUM
+
+        self.accelerator_low_threshold = 0.0
+        for num in range(0, ITER_NUM):
+            data = self.getData()        # 读取传感器信息值
+            ax = data[0][0]
+            ay = data[0][1]
+            az = data[0][2]
+            ax = (ax-self.ax_offset)
+            ay = (ay-self.ay_offset)
+            az = (az-self.az_offset)
+            Raw_accelerationChange = pow(pow(ax, 2)+pow(ay, 2)+pow(az, 2), 0.5)
+            # Multiplied by 10 to values are between 0 to 1
+            self.accelerator_low_threshold += Raw_accelerationChange * 10
+        self.accelerator_low_threshold = (
+            self.accelerator_low_threshold / ITER_NUM)*2
+
+        print('Now you can start tap detection!')
+
+    def detect(self):
+        fall = False
+        data = self.getData()                          # 读取传感器信息值
+        AcX = data[0][0]
+        AcY = data[0][1]
+        AcZ = data[0][2]
+        ax = (AcX-self.ax_offset)
+        ay = (AcY-self.ay_offset)
+        az = (AcZ-self.az_offset)
+        # calculating accelerationChangelitute vector for 3 axis
+        Raw_accelerationChange = pow(pow(ax, 2)+pow(ay, 2)+pow(az, 2), 0.5)
+        # Multiplied by 10 to values are between 0 to 1
+        accelerationChange = Raw_accelerationChange * 10
+        if (fall == True):
+            fall = False
+        if hasattr(self, 'sensitivity'):
+            if 'ACCELERATION_UP_THREADHOLD' in self.sensitivity:
+                self.accelerator_up_threshold = float(
+                    self.sensitivity['ACCELERATION_UP_THREADHOLD'])
+        # Trigger function
+        # if accelerationChange breaks lower threshold
+        if (self.tap_detect_count == 2):
+            if(accelerationChange >= self.accelerator_low_threshold and accelerationChange <= self.accelerator_up_threshold):
+                self.triggercount = self.triggercount+1
+                if (self.triggercount % 3 == 0):  # catch one tap
+                    self.tapcount = self.tapcount+1
+                    utime.sleep_ms(100)
+                if (self.tapcount == self.tap_detect_count):  # catched tap_detect_count
+                    self.triggercount = 0
+                    self.tapcount = 0
+                    fall = True
+            else:
+                self.untriggercount = self.untriggercount+1
+                if (self.untriggercount == 150):
+                    self.triggercount = 0
+                    self.tapcount = 0
+                    self.untriggercount = 0
+        elif (self.tap_detect_count == 1):
+            if(accelerationChange >= self.accelerator_low_threshold and accelerationChange <= self.accelerator_up_threshold):
+                self.triggercount = self.triggercount+1
+                if (self.triggercount % 5 == 0):  # catch one tap
+                    self.tapcount = self.tapcount+1
+                    utime.sleep_ms(100)
+                if (self.tapcount == self.tap_detect_count):  # catched tap_detect_count
+                    self.triggercount = 0
+                    self.tapcount = 0
+                    fall = True
+            else:
+                self.untriggercount = self.untriggercount+1
+                if (self.untriggercount == 150):
+                    self.triggercount = 0
+                    self.tapcount = 0
+                    self.untriggercount = 0
+        return fall

haas_lib_bundles/python/libraries/motion/motion.py

+import utime   # 延时函数在utime
+from .detections.fall_detection import fall_detection
+from .detections.tap_detection import tap_detection
+import _thread      # 线程库
+
+class Motion:
+    def __init__(self, action, getData, onActionDetected):
+        self.action = action
+        if (action == "fall"):
+            self.detectAction = fall_detection(getData)
+        elif (action == "single_tap"):
+            self.detectAction = tap_detection(1, getData)
+        elif (action == "double_tap"):
+            self.detectAction = tap_detection(2, getData)
+        self.onActionDetected = onActionDetected
+        self.enableDetection = False
+        self.detectionThreadCreated = False
+
+    def detect_action(self):
+        while(self.enableDetection == True):
+            isDetected = self.detectAction.detect()                  # 检测Action是否产生
+            if (isDetected == True):                                 # Action被检测到
+                print(self.action, "detected!")
+                if (hasattr(self, 'onActionDetected')):
+                    self.onActionDetected()
+            utime.sleep_us(10)
+        self.detectionThreadCreated = False
+
+    # 使能action检测，若用户不指定灵敏度，则使用默认灵敏度
+    def enable(self, sensitivity=''):
+        if (self.detectionThreadCreated == False):
+            if (sensitivity != ''):
+                self.detectAction.sensitivity = sensitivity
+            try:
+                # 创建行为检测线程
+                self.enableDetection = True
+                self.detectionThreadCreated = True
+                _thread.start_new_thread(self.detect_action, ())
+            except Exception as e:
+                print(e)
+                print("Error: unable to start detect_action thread")
+                self.enableDetection = False
+                self.detectionThreadCreated = False
+
+    # 关闭action检测
+    def disable(self):
+        self.enableDetection = False