Pitch and Yaw from a Wiimote Motion Plus using Cwiid Python

Question

I'm trying to extract the orientation of a wiimote using cwiid in python. I've managed to get the accelerometer values but there doesn't seem to be any object attributes relating to the purely gyroscopic data.

This guy managed to do it in python, but to the best of my knowledge there's no python code online with an example. https://www.youtube.com/watch?v=cUjh0xQO6eY There is information on wiibrew about the controller data but again this seems to be excluded from any python library.

Has anyone got any suggestions? This link has an example of getting gyro data but the packages used don't seem available.

Answer 1

So this question has a few parts, firstly how to extract the gyro data from the motion plus sensor. To do this, the motion plus will first need to be enabled.

The gyro provides the angular rotation vectors, but due to drift caused by integration errors you can't simply use a combination of these two things to get Eular angles. The second part of the question is how to use this data to give position, and that is done by using a Kalman filter, a highly complex matrix sequence, or a complementary filter, a less complex mathematical operation. Both of these filters are essentially combining the gyro and accelerometer data, so as mentioned in a comment above, resulting in more stable measurements, less drift and a system not prone to breaking when the remote is shaken.

Kalman filter: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

Using PyKalman on Raw Acceleration Data to Calculate Position

Complementary filter https://www.instructables.com/Angle-measurement-using-gyro-accelerometer-and-Ar/

Still currently developing the core but will post when I'm finished, hopefully tomorrow. The foundation code I am using to test the measurements is found here: http://andrew-j-norman.blogspot.com/2010/12/more-code.html. Very handy, as it plots the sensor readings automatically after recording. You can see by doing this that even when still, the position estimate by using simple integration of the angular velocities results in a drift in the position vector.

EDIT: Testing this allows for the gyro sensor to accurately calculate the angle changed over time, however there remains drift in the acceleration - which I believe is unavoidable.

Here is an image demonstrating the gyro motion sensor:

Just finished up the code:

#!/usr/bin/python

import cwiid
from time import time, asctime, sleep, perf_counter
from numpy import *
from pylab import *
import math
import numpy as np
from operator import add
HPF = 0.98
LPF = 0.02

def calibrate(wiimote):
    print("Keep the remote still")
    sleep(3)
    print("Calibrating")
    
    messages = wiimote.get_mesg()
    i=0
    accel_init = []
    angle_init = []
    while (i<1000):
        sleep(0.01)
        messages = wiimote.get_mesg()
        for mesg in messages:
            # Motion plus:
            if mesg[0] == cwiid.MESG_MOTIONPLUS:
                if record:
                    angle_init.append(mesg[1]['angle_rate'])
            # Accelerometer:
            elif mesg[0] == cwiid.MESG_ACC:
                if record:
                    accel_init.append(list(mesg[1]))
        i+=1

    accel_init_avg = list(np.mean(np.array(accel_init), axis=0))
    print(accel_init_avg)
    angle_init_avg = sum(angle_init)/len(angle_init)
    print("Finished Calibrating")
    return (accel_init_avg, angle_init_avg)
    
def plotter(plot_title, timevector, data, position, n_graphs):
   subplot(n_graphs, 1, position)
   plot(timevector, data[0], "r",
        timevector, data[1], "g",
        timevector, data[2], "b")
   xlabel("time (s)")
   ylabel(plot_title)

print("Press 1+2 on the Wiimote now")
wiimote = cwiid.Wiimote()

# Rumble to indicate a connection
wiimote.rumble = 1
print("Connection established - release buttons")
sleep(0.2)
wiimote.rumble = 0
sleep(1.0)

wiimote.enable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS)
wiimote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC | cwiid.RPT_MOTIONPLUS

accel_init, angle_init = calibrate(wiimote)
str = ""
print("Press plus to start recording, minus to end recording")
loop = True
record = False
accel_data = []
angle_data = []
messages = wiimote.get_mesg()
while (loop):
   sleep(0.01)
   messages = wiimote.get_mesg()
   for mesg in messages:
       # Motion plus:
       if mesg[0] == cwiid.MESG_MOTIONPLUS:
           if record:
               angle_data.append({"Time" : perf_counter(), \
                   "Rate" : mesg[1]['angle_rate']})
       # Accelerometer:
       elif mesg[0] == cwiid.MESG_ACC:
           if record:               
               accel_data.append({"Time" : perf_counter(), "Acc" : [mesg[1][i] - accel_init[i] for i in range(len(accel_init))]})
       # Button:
       elif mesg[0] == cwiid.MESG_BTN:
           if mesg[1] & cwiid.BTN_PLUS and not record:
               print("Recording - press minus button to stop")
               record = True
               start_time = perf_counter()
           if mesg[1] & cwiid.BTN_MINUS and record:
               if len(accel_data) == 0:
                   print("No data recorded")
               else:
                   print("End recording")
                   print("{0} data points in {1} seconds".format(
                       len(accel_data), perf_counter() - accel_data[0]["Time"]))
               record = False
               loop = False
       else:
           pass

wiimote.disable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS)
if len(accel_data) == 0:
   sys.exit()


timevector = []
a = [[],[],[]]
v = [[],[],[]]
p = [[],[],[]]
last_time = 0
velocity = [0,0,0]
position = [0,0,0]

for n, x in enumerate(accel_data):
   if (n == 0):
       origin = x
   else:
       elapsed = x["Time"] - origin["Time"]
       delta_t = x["Time"] - last_time
       timevector.append(elapsed)
       for i in range(3):
           acceleration = x["Acc"][i] - origin["Acc"][i]
           velocity[i] = velocity[i] + delta_t * acceleration
           position[i] = position[i] + delta_t * velocity[i]
           a[i].append(acceleration)
           v[i].append(velocity[i])
           p[i].append(position[i])
   last_time = x["Time"]

n_graphs = 3
if len(angle_data) == len(accel_data):
   n_graphs = 5
   angle_accel = [(math.pi)/2 if (j**2 + k**2)==0 else math.atan(i/math.sqrt(j**2 + k**2)) for i,j,k in zip(a[0],a[1],a[2])]
   ar = [[],[],[]] # Angle rates
   aa = [[],[],[]] # Angles
   angle = [0,0,0]
   for n, x in enumerate(angle_data):
       if (n == 0):
           origin = x
       else:
           delta_t = x["Time"] - last_time
           for i in range(3):
               rate = x["Rate"][i] - origin["Rate"][i]
               angle[i] = HPF*(np.array(angle[i]) + delta_t * rate) + LPF*np.array(angle_accel)
               ar[i].append(rate)
               aa[i].append(angle[i])
       last_time = x["Time"]


plotter("Acceleration", timevector, a, 1, n_graphs)
if n_graphs == 5:
   plotter("Angle Rate", timevector, ar, 4, n_graphs)
   plotter("Angle", timevector, aa, 5, n_graphs)

show()

Answer 2

I was actually looking for this a few days ago, found this post: https://ofalcao.pt/blog/2014/controlling-the-sbrick-with-a-wiimote. More specifically, I think the code you're looking for is:

# roll = accelerometer[0], standby ~125
# pitch = accelerometer[1], standby ~125

...

    roll=(wm.state['acc'][0]-125)
    pitch=(wm.state['acc'][1]-125)

I'm assuming you can use the z-axis (index 2) for the yaw