Raspberry Pi で USB 接続の加速度・角速度センサーを使ってモーターの振動を取得 ＆ AWS IoT Core に送ってみた

$ lsb_release -a
No LSB modules are available.
Distributor ID:	Debian
Description:	Debian GNU/Linux 11 (bullseye)
Release:	11
Codename:	bullseye

usb.core.USBError: [Errno 13] Access denied (insufficient permissions)

$ lsusb

Bus 001 Device 003: ID 07c0:111a Code Mercenaries Hard- und Software GmbH JoyWarrior56FR1

SUBSYSTEM=="usb", ATTR{idVendor}=="07c0", ATTR{idProduct}=="111a", MODE="0666", GROUP="plugdev"

sudo udevadm control --reload-rules && sudo udevadm trigger

#!/usr/bin/python3

import csv
import datetime
import struct
import sys

import usb.core
import usb.util

VID = 0x07c0
PID = 0x111a
ARNG = 0x00
AFILT = 0xC3
MODE = 0x00
RANGE_ACC = 2

def cal_acceleration(n):
    return round((n - 32768) / 32768 * RANGE_ACC * 9.80665, 3)

if __name__ == "__main__":
    dev = usb.core.find(idVendor=VID, idProduct=PID)

    if dev is None:
        raise ValueError("Device not found")

    try:
        dev.detach_kernel_driver(0)
    except Exception:
        pass

    dev.write(2, [0x04, MODE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
    dev.write(2, [0x00, ARNG, AFILT, 0x00, 0x00, 0x00, 0x00, 0x00])
    dev.write(2, [0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])

    cfg = dev.get_active_configuration()
    intf = cfg[(0, 0)]
    if dev.is_kernel_driver_active(intf.bInterfaceNumber):
        dev.detach_kernel_driver(intf.bInterfaceNumber)

    endpoint = usb.util.find_descriptor(
        intf,
        custom_match=lambda e: usb.util.endpoint_direction(e.bEndpointAddress)
        == usb.util.ENDPOINT_IN
    )

    if endpoint is None:
        raise ValueError('Endpoint not found')

    with open("./JoyWarrior56FR1_test.csv", "w", encoding="utf_8_sig") as f:
        writer = csv.writer(f)
        writer.writerow(["日時", "加速度x", "加速度y", "加速度z"])
        while True:
            output_bytes = endpoint.read(endpoint.wMaxPacketSize).tobytes()
            acc_bytes = struct.unpack("<HHH", output_bytes[:6])
            accx, accy, accz = list(map(cal_acceleration, acc_bytes))
            timestamp = datetime.datetime.now().strftime("%Y%m%d%H%M%S.%f")
            print(f"加速度x[m/s^2]: {accx} 加速度y[m/s^2]: {accy} 加速度z[m/s^2]: {accz}")
            writer.writerow([timestamp, accx, accy, accz])
            f.flush()

python JoyWarrior56FR1.py

日時,加速度x,加速度y,加速度z
1725369745.2017078,-0.028,-0.013,0.028
1725369745.238774,-0.079,-0.876,1.669
1725369745.2416916,-2.395,-0.598,0.006
・・・

加速度x[m/s^2]: -0.479 加速度y[m/s^2]: -0.599 加速度z[m/s^2]: 0.402
加速度x[m/s^2]: 9.831 加速度y[m/s^2]: 0.512 加速度z[m/s^2]: 0.761
加速度x[m/s^2]: -2.332 加速度y[m/s^2]: 0.029 加速度z[m/s^2]: -0.096
加速度x[m/s^2]: -2.241 加速度y[m/s^2]: -0.589 加速度z[m/s^2]: 0.752
加速度x[m/s^2]: 9.668 加速度y[m/s^2]: 1.796 加速度z[m/s^2]: -4.501
・・・

加速度x[m/s^2]: 4.079 加速度y[m/s^2]: 0.009 加速度z[m/s^2]: 1.044
加速度x[m/s^2]: -1.548 加速度y[m/s^2]: 0.198 加速度z[m/s^2]: -1.034
加速度x[m/s^2]: -5.286 加速度y[m/s^2]: -0.851 加速度z[m/s^2]: 1.92
加速度x[m/s^2]: 3.97 加速度y[m/s^2]: 0.097 加速度z[m/s^2]: 0.371
加速度x[m/s^2]: -1.171 加速度y[m/s^2]: 0.394 加速度z[m/s^2]: -1.489

#!/usr/bin/python3
from awscrt import io, mqtt, auth, http
from awsiot import mqtt_connection_builder

import csv
import struct
import sys
import usb.core
import usb.util
import time
import json
import logging
import RPi.GPIO as GPIO

# Define ENDPOINT, CLIENT_ID, PATH_TO_CERTIFICATE, PATH_TO_PRIVATE_KEY, PATH_TO_AMAZON_ROOT_CA_1, MESSAGE, TOPIC, and RANGE
ENDPOINT = "[YOUR_AWS_IoT_SUB_DOMAIN].iot.ap-northeast-1.amazonaws.com"
CLIENT_ID = "[YOUR_CLIENT_ID]"
PATH_TO_CERTIFICATE = "[YOUR_CERTIFICATE_PATH]"
PATH_TO_PRIVATE_KEY = "[YOUR_PRIVATE_KEY_PATH]"
PATH_TO_AMAZON_ROOT_CA_1 = "[AMAZON_ROOT_CA_1_PATH]"
#MESSAGE = "Hello World"
#TOPIC_PUB = "osaka/swtest02/motion/pub"
#TOPIC_SUB = "osaka/swtest02/sub/flinkresult"
TOPIC_PUB = "blog/pub/test"
TOPIC_SUB = "blog/sub/test"
RANGE = 20

# Logger Settings
logger = logging.getLogger()
logger.setLevel(logging.INFO)
streamHandler = logging.StreamHandler(stream=sys.stdout)
formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
streamHandler.setFormatter(formatter)
logger.addHandler(streamHandler)

# Spin up resources
event_loop_group = io.EventLoopGroup(1)
host_resolver = io.DefaultHostResolver(event_loop_group)
client_bootstrap = io.ClientBootstrap(event_loop_group, host_resolver)
mqtt_connection = mqtt_connection_builder.mtls_from_path(
            endpoint=ENDPOINT,
            cert_filepath=PATH_TO_CERTIFICATE,
            pri_key_filepath=PATH_TO_PRIVATE_KEY,
            client_bootstrap=client_bootstrap,
            ca_filepath=PATH_TO_AMAZON_ROOT_CA_1,
            client_id=CLIENT_ID,
            clean_session=False,
            keep_alive_secs=6
            )
print("Connecting to {} with client ID '{}'...".format(
        ENDPOINT, CLIENT_ID))
# Make the connect() call
connect_future = mqtt_connection.connect()
# Future.result() waits until a result is available
connect_future.result()
print("Connected!")

# JoyWarrior Settings
VID = 0x07c0
PID = 0x111a
ARNG = 0x00
AFILT = 0xC3
MODE = 0x00
#MODE = 0x01
RANGE_ACC = 2

# GPIO Settings
GPIO.setmode(GPIO.BCM)
GPIO.setup(3, GPIO.OUT)

def cal_acceleration(n):
    return round((n - 32768) / 32768 * RANGE_ACC * 9.80665, 3)

# Main Logic
if __name__ == "__main__":
    dev = usb.core.find(idVendor=VID, idProduct=PID)

    if dev is None:
        raise ValueError("Device not found")

    try:
        dev.detach_kernel_driver(0)
    except Exception:
        pass

    dev.write(2, [0x04, MODE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])
    dev.write(2, [0x00, ARNG, AFILT, 0x00, 0x00, 0x00, 0x00, 0x00])
    dev.write(2, [0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00])

    cfg = dev.get_active_configuration()
    intf = cfg[(0, 0)]
    if dev.is_kernel_driver_active(intf.bInterfaceNumber):
        dev.detach_kernel_driver(intf.bInterfaceNumber)

    endpoint = usb.util.find_descriptor(
        intf,
        custom_match=lambda e: usb.util.endpoint_direction(e.bEndpointAddress)
        == usb.util.ENDPOINT_IN
    )

    if endpoint is None:
        raise ValueError('Endpoint not found')

    with open("./aws_iot_test.csv", "w", encoding="utf_8_sig") as f:
        writer = csv.writer(f)
        writer.writerow(["日時", "加速度x", "加速度y", "加速度z"])
        while True:
            output_bytes = endpoint.read(endpoint.wMaxPacketSize).tobytes()
            acc_bytes = struct.unpack("<HHH", output_bytes[:6])
            accx, accy, accz = list(map(cal_acceleration, acc_bytes))
            timestamp = int(time.time())

            payload = {
                "timestamp": timestamp,
                "accx": accx,
                "accy": accy,
                "accz": accz
            }

            mqtt_connection.publish(topic=TOPIC_PUB, payload=json.dumps(payload), qos=mqtt.QoS.AT_LEAST_ONCE)
            print("Published: '" + json.dumps(payload) + "' to the topic: " + TOPIC_PUB)
            writer.writerow([timestamp, accx, accy, accz])
            f.flush()
            time.sleep(0.2)