Accelerometer and magnetometer (Bosch BNO055)


ouzle

Very Active Member
Joined
Jun 3, 2019
Messages
130
Location
England
I started playing with the accelerometer and magnetometer today. I've got a text only bubble-level working that could be the basis of a graphical bubble level. We can reset the horizontal (or any base surface) with another script. I'll share the scripts first and then describe how I got the useful numbers from the sensors.

EDIT (21 July 2021) - Updated order of offset and scaling based on @chidochidochido 's findings. The offset was zero on my pyra so there is no change in the results.

The pyra has a Bosch BNO055 chip. I don't know how to change settings on the sensor chip from user space yet so I'm just processing values from /sys/bus/iio/devices/iio\:device1/ here.


To use the bash scripts, unzip the attachment to your home directory. You can then execute the script from a terminal with:

Bash:
cd ~/bno055

watch -n0.2 ./pyra-bno055

./pyra-bubble-reset

watch -n0.2 ./pyra-bubble


The first script shows how to find the BNO055 driver output in the /sys folder and how to read and scale all the values. The units look to be set to [m/s^2], [rad/s], and micro tesla by default, but this can be configured on the chip.


Bash:
me@pyra:~/bno055$ ./pyra-bno055

Acceleration -0.15 -0.18 -9.88

Rotation     0.01 -0.02 -0.01

Magnetometer 121.34 15.33 122.83


Bash:
#!/usr/bin/bash

# Find the SYSFS path the ino055 sensor chip
SYSNAME=
for d in "/sys/bus/iio/devices/iio*/name" ; do
    SYSNAME=$(grep -l bno055 $d)
    if [ -f $SYSNAME ]; then break; fi
done

if [ ! -f $SYSNAME ] ; then
    echo "bno055 sensor chip not found in /sys/bus/iio/devices"
    exit 1
fi
SYS=$(dirname $SYSNAME)

# Read the acceleration
ascale=$(cat $SYS/in_accel_scale)
ax0=$(cat $SYS/in_accel_x_offset)
ay0=$(cat $SYS/in_accel_y_offset)
az0=$(cat $SYS/in_accel_z_offset)
axr=$(cat $SYS/in_accel_x_raw)
ayr=$(cat $SYS/in_accel_y_raw)
azr=$(cat $SYS/in_accel_z_raw)
accel_x=$(awk "BEGIN{ printf(\"%.2f\",($axr+$ax0)*$ascale) }")
accel_y=$(awk "BEGIN{ printf(\"%.2f\",($ayr+$ay0)*$ascale) }")
accel_z=$(awk "BEGIN{ printf(\"%.2f\",($azr+$az0)*$ascale) }")

# Read the angular velocity
avscale=$(cat $SYS/in_anglvel_scale)
avx0=$(cat $SYS/in_anglvel_x_offset)
avy0=$(cat $SYS/in_anglvel_y_offset)
avz0=$(cat $SYS/in_anglvel_z_offset)
avxr=$(cat $SYS/in_anglvel_x_raw)
avyr=$(cat $SYS/in_anglvel_y_raw)
avzr=$(cat $SYS/in_anglvel_z_raw)
avx=$(awk "BEGIN{ printf(\"%.2f\",($avxr+$avx0)*$avscale) }")
avy=$(awk "BEGIN{ printf(\"%.2f\",($avyr+$avy0)*$avscale) }")
avz=$(awk "BEGIN{ printf(\"%.2f\",($avzr+$avz0)*$avscale) }")

# Read the magnetometer
mscale=$(cat $SYS/in_magn_scale)
mx0=$(cat $SYS/in_magn_x_offset)
my0=$(cat $SYS/in_magn_y_offset)
mz0=$(cat $SYS/in_magn_z_offset)
mxr=$(cat $SYS/in_magn_x_raw)
myr=$(cat $SYS/in_magn_y_raw)
mzr=$(cat $SYS/in_magn_z_raw)
mx=$(awk "BEGIN{ printf(\"%.2f\",($mxr+$mx0)*$mscale) }")
my=$(awk "BEGIN{ printf(\"%.2f\",($myr+$my0)*$mscale) }")
mz=$(awk "BEGIN{ printf(\"%.2f\",($mzr+$mz0)*$mscale) }")

echo "Acceleration $accel_x $accel_y $accel_z"
echo "Rotation     $avx $avy $avz"
echo "Magnetometer $mx $my $mz"


The next script reports two numbers that are between -1.0 and +1.0. These are like the X, Y coordinate of a bubble in a bubble level. When the bubble is level, they are both zero.

Bash:
#!/usr/bin/bash

# A bubble level using the pyra's bno055 accelerometer
# Call pyra-bubble-reset to reset the bubble offset
MATRIX="${HOME}/.bno055-bubble"

# Find the SYSFS path the ino055 sensor chip
SYSNAME=
for d in "/sys/bus/iio/devices/iio*/name" ; do
    SYSNAME=$(grep -l bno055 $d)
    if [ -f $SYSNAME ]; then break; fi
done

if [ ! -f $SYSNAME ] ; then
    echo "bno055 sensor chip not found in /sys/bus/iio/devices"
    exit 1
fi
SYS=$(dirname $SYSNAME)

# Read the acceleration
ascale=$(cat $SYS/in_accel_scale)
ax0=$(cat $SYS/in_accel_x_offset)
ay0=$(cat $SYS/in_accel_y_offset)
az0=$(cat $SYS/in_accel_z_offset)
axr=$(cat $SYS/in_accel_x_raw)
ayr=$(cat $SYS/in_accel_y_raw)
azr=$(cat $SYS/in_accel_z_raw)
a[0]=$(awk "BEGIN{ printf(\"%.2f\",($axr+$ax0)*$ascale) }")
a[1]=$(awk "BEGIN{ printf(\"%.2f\",($ayr+$ay0)*$ascale) }")
a[2]=$(awk "BEGIN{ printf(\"%.2f\",($azr+$az0)*$ascale) }")

# Read the rotation matrix, or use the default
R[00]=1 ; R[01]=0 ; R[02]=0
R[10]=0 ; R[11]=1 ; R[12]=0
R[20]=0 ; R[21]=0 ; R[22]=1

if [ -f ${MATRIX} ] ; then
    row=0
    while IFS= read -r line; do
        if [ $row -eq 0 ]; then 
            R[00]=$(echo $line | cut -d ' ' -f 1)
            R[01]=$(echo $line | cut -d ' ' -f 2)
            R[02]=$(echo $line | cut -d ' ' -f 3)
        elif [ $row -eq 1 ]; then 
            R[10]=$(echo $line | cut -d ' ' -f 1)
            R[11]=$(echo $line | cut -d ' ' -f 2)
            R[12]=$(echo $line | cut -d ' ' -f 3)
        elif [ $row -eq 2 ]; then 
            R[20]=$(echo $line | cut -d ' ' -f 1)
            R[21]=$(echo $line | cut -d ' ' -f 2)
            R[22]=$(echo $line | cut -d ' ' -f 3)
        fi
        row=$((row+1))
    done < ${MATRIX}
fi
#echo ${R[00]} ${R[01]} ${R[02]}
#echo ${R[10]} ${R[11]} ${R[12]}
#echo ${R[20]} ${R[21]} ${R[22]}

# Rotate the acceleration vector
a[0]=$(awk "BEGIN{ printf(\"%.2f\",${a[0]}*${R[00]} + ${a[1]}*${R[01]} + ${a[2]}*${R[02]} ) }")
a[1]=$(awk "BEGIN{ printf(\"%.2f\",${a[0]}*${R[10]} + ${a[1]}*${R[11]} + ${a[2]}*${R[12]} ) }")
a[2]=$(awk "BEGIN{ printf(\"%.2f\",${a[0]}*${R[20]} + ${a[1]}*${R[21]} + ${a[2]}*${R[22]} ) }")

# Usage: normalize v[@]
# Result stored in normal[0], normal[1], normal[2]
function normalize
{
    # Declare first parameter as indexed array
    declare -a v1=("${!1}")

    mag=$(awk "BEGIN{ printf(\"%f\", sqrt(${v1[0]}*${v1[0]} + ${v1[1]}*${v1[1]} + ${v1[2]}*${v1[2]}) ) }")   
    normal[0]=$(awk "BEGIN{ printf(\"%.2f\", ${v1[0]}/$mag ) }")
    normal[1]=$(awk "BEGIN{ printf(\"%.2f\", ${v1[1]}/$mag ) }")
    normal[2]=$(awk "BEGIN{ printf(\"%.2f\", ${v1[2]}/$mag ) }")
}

normalize a[@]
echo ${normal[0]} ${normal[1]}


The last script is used to reset the bubble level to horizontal or vertical as you like. Put the pyra on a reference surface then run the script.

Bash:
#!/usr/bin/bash

# Measures the direction of down and creates a rotation matrix
# that can be used to correct the down direction later
MATRIX="${HOME}/.bno055-bubble"

# Find the SYSFS path the ino055 sensor chip
SYSNAME=
for d in "/sys/bus/iio/devices/iio*/name" ; do
    SYSNAME=$(grep -l bno055 $d)
    if [ -f $SYSNAME ]; then break; fi
done

if [ ! -f $SYSNAME ] ; then
    echo "bno055 sensor chip not found in /sys/bus/iio/devices"
    exit 1
fi
SYS=$(dirname $SYSNAME)

# Read the acceleration
ascale=$(cat $SYS/in_accel_scale)
ax0=$(cat $SYS/in_accel_x_offset)
ay0=$(cat $SYS/in_accel_y_offset)
az0=$(cat $SYS/in_accel_z_offset)
axr=$(cat $SYS/in_accel_x_raw)
ayr=$(cat $SYS/in_accel_y_raw)
azr=$(cat $SYS/in_accel_z_raw)
a[0]=$(awk "BEGIN{ printf(\"%.2f\",($axr+$ax0)*$ascale) }")
a[1]=$(awk "BEGIN{ printf(\"%.2f\",($ayr+$ay0)*$ascale) }")
a[2]=$(awk "BEGIN{ printf(\"%.2f\",($azr+$az0)*$ascale) }")

# Usage: normalize v[@]
# Result stored in normal[0], normal[1], normal[2]
function normalize
{
    # Declare first parameter as indexed array
    declare -a v1=("${!1}")

    mag=$(awk "BEGIN{ printf(\"%f\", sqrt(${v1[0]}*${v1[0]} + ${v1[1]}*${v1[1]} + ${v1[2]}*${v1[2]}) ) }")   
    normal[0]=$(awk "BEGIN{ printf(\"%f\", ${v1[0]}/$mag ) }")
    normal[1]=$(awk "BEGIN{ printf(\"%f\", ${v1[1]}/$mag ) }")
    normal[2]=$(awk "BEGIN{ printf(\"%f\", ${v1[2]}/$mag ) }")
}

# Usage: normalize v1[@] v2[@]
# Result stored in dot[0], dot[1], dot[2]
function dotProduct
{
    declare -a v1=("${!1}")
    declare -a v2=("${!2}")

    dot=$(awk "BEGIN{ printf(\"%f\", ${v1[0]}*${v2[0]} + ${v1[1]}*${v2[1]} + ${v1[2]}*${v2[2]}) }")
}

# Usage: normalize v1[@] v2[@]
# Result stored in cross[0], cross[1], cross[2]
function crossProduct
{
    declare -a v1=("${!1}")
    declare -a v2=("${!2}")

    cross[0]=$(awk "BEGIN{ a=${v1[1]}*${v2[2]}; b=${v1[2]}*${v2[1]}; printf(\"%f\", a - b) }")
    cross[1]=$(awk "BEGIN{ a=${v1[0]}*${v2[2]}; b=${v1[2]}*${v2[0]}; printf(\"%f\", b - a) }")
    cross[2]=$(awk "BEGIN{ a=${v1[0]}*${v2[1]}; b=${v1[1]}*${v2[0]}; printf(\"%f\", a - b) }")
}

function rotateAlign
{      
    k=$(awk "BEGIN{ printf(\"%f\", 1.0 / ( 1.0 + ${dot} ) )}")       
    R[00]=$(awk "BEGIN{ printf(\"%f\", ${cross[0]} * ${cross[0]} * ${k} + ${dot}      )}")
    R[01]=$(awk "BEGIN{ printf(\"%f\", ${cross[0]} * ${cross[1]} * ${k} + ${cross[2]} )}") 
    R[02]=$(awk "BEGIN{ printf(\"%f\", ${cross[0]} * ${cross[2]} * ${k} - ${cross[1]} )}") 
    R[10]=$(awk "BEGIN{ printf(\"%f\", ${cross[1]} * ${cross[0]} * ${k} - ${cross[2]} )}")
    R[11]=$(awk "BEGIN{ printf(\"%f\", ${cross[1]} * ${cross[1]} * ${k} + ${dot}      )}")     
    R[12]=$(awk "BEGIN{ printf(\"%f\", ${cross[1]} * ${cross[2]} * ${k} + ${cross[0]} )}") 
    R[20]=$(awk "BEGIN{ printf(\"%f\", ${cross[2]} * ${cross[0]} * ${k} + ${cross[1]} )}")
    R[21]=$(awk "BEGIN{ printf(\"%f\", ${cross[2]} * ${cross[1]} * ${k} - ${cross[0]} )}")
    R[22]=$(awk "BEGIN{ printf(\"%f\", ${cross[2]} * ${cross[2]} * ${k} + ${dot}      )}")
}

# Calculate the rotation matrix needed to make the vector a point upwards
up[0]=0
up[1]=0
up[2]=-1

normalize a[@]
dotProduct up[@] normal[@]
crossProduct up[@] normal[@]
rotateAlign

#echo "magnitude: $mag"
#echo "normal: ${normal[0]} ${normal[1]} ${normal[2]}"
#echo "dot: $dot"
#echo "cross: ${cross[0]} ${cross[1]} ${cross[2]}"

echo
echo Saving matrix to ${MATRIX}
echo ${R[00]} ${R[01]} ${R[02]} > ${MATRIX}
echo ${R[10]} ${R[11]} ${R[12]} >> ${MATRIX}
echo ${R[20]} ${R[21]} ${R[22]} >> ${MATRIX}
cat ${MATRIX}
 

Attachments

  • pyra-bno055.zip
    6.4 KB · Views: 3
Last edited:

ouzle

Very Active Member
Joined
Jun 3, 2019
Messages
130
Location
England
Here are two scripts to turn the pyra into a compass. This was more difficult because of the calibration calculations. I get good result as long as I put the pyra down while it is giving a compass reading.

The scripts are attached to the opening post or you can read them here.

EDIT (21 July 2021) - Swapped order of offset and scaling, no change in results here though.

Python:
#!/usr/bin/env python3

# Collects readings from the magnetometer while the user rotates the pyra.
# The magnetometer vectors are on the surface of a sphere.
# The centre of the sphere is the compass offset and the radius is the strength of earths magnetic field.
# The least squares method is used to fit a sphere to a sample of vectors.
# The radius and origin of the sphere is saved to a configuration file.
 
import numpy as np
import os
import sys
import time
import math
import subprocess

bno055cmd="./pyra-bno055"
bno055compass="~/.bno055-compass"

#    fit a sphere to X,Y, and Z data points
#    returns the radius and center points of
#    the best fit sphere
def sphereFit(spX,spY,spZ):
    #   Assemble the A matrix
    spX = np.array(spX)
    spY = np.array(spY)
    spZ = np.array(spZ)
    A = np.zeros((len(spX),4))
    A[:,0] = spX*2
    A[:,1] = spY*2
    A[:,2] = spZ*2
    A[:,3] = 1

    #   Assemble the f matrix
    f = np.zeros((len(spX),1))
    f[:,0] = (spX*spX) + (spY*spY) + (spZ*spZ)
    C, residules, rank, singval = np.linalg.lstsq(A,f,rcond=None)

    #   solve for the radius
    t = (C[0]*C[0])+(C[1]*C[1])+(C[2]*C[2])+C[3]
    radius = math.sqrt(t)

    return radius, C[0][0], C[1][0], C[2][0]

def main():
    print("Compass calibration")
    print("   Rotate the pyra in a complex motion")
    print("   Including figure of 8s and full rotations.")
    input("Press any key to start...")


    # Sample the magnetometer
    samples_x=[]
    samples_y=[]
    samples_z=[] 
    
    start_time = time.time()
    duration=10
    remaining=duration
    while remaining>0:   
        remaining=duration-(time.time()-start_time)
        print("{}   ".format(int(remaining)),end="\r")

        try:
            bno055output = subprocess.check_output(bno055cmd, shell=True)
            lines = bno055output.splitlines()
            for line in lines:
                if b"Magnetometer" in line:
                    col = line.split()
                    samples_x.append(float(col[1].strip()))
                    samples_y.append(float(col[2].strip()))
                    samples_z.append(float(col[3].strip()))

        except:
            print ("Could not parse output from {}".format(bno055cmd))
            sys.exit(1)
    
    print( "Collected {} samples".format(len(samples_x)))
        
    if len(samples_x)<30:
        print ("Not enough magnetometer samples collected")
        sys.exit(1)
        
    # Fit a sphere to the magnetic field vectors
    radius, cx, cy, cz = sphereFit(samples_x,samples_y,samples_z)
    print()
    print('Field strength={:.2f}uT'.format(radius))
    print('Offset={0:.2f},{1:.2f},{2:.2f}'.format(cx,cy,cz))
    
    if radius<2 or radius>20:
        print("Not enough change in the magnetic field to calibrate.")
        
    # Save the result
    path = os.path.expanduser(bno055compass)
    text_file = open(path, "w")
    with open(path, "w") as text_file:
        text_file.write("{} {} {} {}\n".format(radius,cx,cy,cz))

    text_file.close()
if __name__== "__main__":
  main()


Bash:
#!/usr/bin/bash
# A compass using the pyra's bno055 magnetmeter
# Call pyra-compass-reset first to calibrate the compass

COMPASS_CALIBRATION="${HOME}/.bno055-compass"
NUMBER_OF_SAMPLES=10

if ! [ -f ${COMPASS_CALIBRATION} ] ; then
    echo "Call pyra-compass-reset first to calibrate the compass"
    exit 1
fi

# Find the SYSFS path the bno055 sensor chip
SYSNAME=
for d in "/sys/bus/iio/devices/iio*/name" ; do 
    SYSNAME=$(grep -l bno055 $d)
    if [ -f $SYSNAME ]; then break; fi
done

if [ ! -f $SYSNAME ] ; then
    echo "bno055 sensor chip not found in /sys/bus/iio/devices"
    exit 1
fi
SYS=$(dirname $SYSNAME)

# Read the accelerometer scale
ascale=$(cat $SYS/in_accel_scale)
ax0=$(cat $SYS/in_accel_x_offset)
ay0=$(cat $SYS/in_accel_y_offset)
az0=$(cat $SYS/in_accel_z_offset)

# Read the magnetometer scale
mscale=$(cat $SYS/in_magn_scale)
mx0=$(cat $SYS/in_magn_x_offset)
my0=$(cat $SYS/in_magn_y_offset)
mz0=$(cat $SYS/in_magn_z_offset)

# Average several readings from the sensors
mxa=0
mya=0
mza=0
axa=0
aya=0
aza=0
for i in $(seq 1 ${NUMBER_OF_SAMPLES}); do
    # Collect the raw readings
    mxr=$(cat $SYS/in_magn_x_raw)
    myr=$(cat $SYS/in_magn_y_raw)
    mzr=$(cat $SYS/in_magn_z_raw)    
    axr=$(cat $SYS/in_accel_x_raw)
    ayr=$(cat $SYS/in_accel_y_raw)
    azr=$(cat $SYS/in_accel_z_raw)
    
    # Scale the readings
    mx=$(awk "BEGIN{ printf(\"%f\",($mxr+$mx0)*$mscale) }")
    my=$(awk "BEGIN{ printf(\"%f\",($myr+$my0)*$mscale) }")
    mz=$(awk "BEGIN{ printf(\"%f\",($mzr+$mz0)*$mscale) }")
    ax=$(awk "BEGIN{ printf(\"%f\",($axr+$ax0)*$ascale) }")
    ay=$(awk "BEGIN{ printf(\"%f\",($ayr+$ay0)*$ascale) }")
    az=$(awk "BEGIN{ printf(\"%f\",($azr+$az0)*$ascale) }")

    # Sum the readings for the average
    mxa=$(awk "BEGIN{ printf(\"%f\",$mxa + $mx) }")
    mya=$(awk "BEGIN{ printf(\"%f\",$mya + $my) }")
    mza=$(awk "BEGIN{ printf(\"%f\",$mza + $mz) }")        
    axa=$(awk "BEGIN{ printf(\"%f\",$axa + $ax) }")
    aya=$(awk "BEGIN{ printf(\"%f\",$aya + $ay) }")
    aza=$(awk "BEGIN{ printf(\"%f\",$aza + $az) }")        

    # Give the sensor chance to update
    #sleep 0.05
done
mx=$(awk "BEGIN{ printf(\"%f\",$mxa/$i) }")
my=$(awk "BEGIN{ printf(\"%f\",$mya/$i) }")
mz=$(awk "BEGIN{ printf(\"%f\",$mza/$i) }")
a[0]=$(awk "BEGIN{ printf(\"%f\",$axa/$i) }")
a[1]=$(awk "BEGIN{ printf(\"%f\",$aya/$i) }")
a[2]=$(awk "BEGIN{ printf(\"%f\",$aza/$i) }")

# Usage: normalize v[@]
# Result stored in normal[0], normal[1], normal[2]
function normalize 
{
    # Declare first parameter as indexed array
    declare -a v1=("${!1}") 

    mag=$(awk "BEGIN{ printf(\"%f\", sqrt(${v1[0]}*${v1[0]} + ${v1[1]}*${v1[1]} + ${v1[2]}*${v1[2]}) ) }")    
    normal[0]=$(awk "BEGIN{ printf(\"%f\", ${v1[0]}/$mag ) }")
    normal[1]=$(awk "BEGIN{ printf(\"%f\", ${v1[1]}/$mag ) }")
    normal[2]=$(awk "BEGIN{ printf(\"%f\", ${v1[2]}/$mag ) }")
}

# Usage: normalize v1[@] v2[@]
# Result stored in dot[0], dot[1], dot[2]
function dotProduct 
{
    declare -a v1=("${!1}")
    declare -a v2=("${!2}") 

    dot=$(awk "BEGIN{ printf(\"%f\", ${v1[0]}*${v2[0]} + ${v1[1]}*${v2[1]} + ${v1[2]}*${v2[2]}) }")
}

# Usage: normalize v1[@] v2[@]
# Result stored in cross[0], cross[1], cross[2]
function crossProduct
{
    declare -a v1=("${!1}")
    declare -a v2=("${!2}") 

    cross[0]=$(awk "BEGIN{ a=${v1[1]}*${v2[2]}; b=${v1[2]}*${v2[1]}; printf(\"%f\", a - b) }")
    cross[1]=$(awk "BEGIN{ a=${v1[0]}*${v2[2]}; b=${v1[2]}*${v2[0]}; printf(\"%f\", b - a) }")
    cross[2]=$(awk "BEGIN{ a=${v1[0]}*${v2[1]}; b=${v1[1]}*${v2[0]}; printf(\"%f\", a - b) }")
}

function rotateAlign
{       
    k=$(awk "BEGIN{ printf(\"%f\", 1.0 / ( 1.0 + ${dot} ) )}")        
    R[00]=$(awk "BEGIN{ printf(\"%f\", ${cross[0]} * ${cross[0]} * ${k} + ${dot}      )}")
    R[01]=$(awk "BEGIN{ printf(\"%f\", ${cross[0]} * ${cross[1]} * ${k} + ${cross[2]} )}")  
    R[02]=$(awk "BEGIN{ printf(\"%f\", ${cross[0]} * ${cross[2]} * ${k} - ${cross[1]} )}")  
    R[10]=$(awk "BEGIN{ printf(\"%f\", ${cross[1]} * ${cross[0]} * ${k} - ${cross[2]} )}")
    R[11]=$(awk "BEGIN{ printf(\"%f\", ${cross[1]} * ${cross[1]} * ${k} + ${dot}      )}")      
    R[12]=$(awk "BEGIN{ printf(\"%f\", ${cross[1]} * ${cross[2]} * ${k} + ${cross[0]} )}")  
    R[20]=$(awk "BEGIN{ printf(\"%f\", ${cross[2]} * ${cross[0]} * ${k} + ${cross[1]} )}")
    R[21]=$(awk "BEGIN{ printf(\"%f\", ${cross[2]} * ${cross[1]} * ${k} - ${cross[0]} )}")
    R[22]=$(awk "BEGIN{ printf(\"%f\", ${cross[2]} * ${cross[2]} * ${k} + ${dot}      )}")
}

# Calculate the rotation matrix needed to make the acceleration vector a point upwards
up[0]=0
up[1]=0
up[2]=1

normalize a[@]
dotProduct normal[@] up[@]
crossProduct normal[@] up[@] 
rotateAlign

# Read the compass calibration
cscale=$(awk -F' ' 'NR=1 {print $1}' ${COMPASS_CALIBRATION})
cx0=$(awk -F' ' 'NR=1 {print $2}' ${COMPASS_CALIBRATION})
cy0=$(awk -F' ' 'NR=1 {print $3}' ${COMPASS_CALIBRATION})
cz0=$(awk -F' ' 'NR=1 {print $4}' ${COMPASS_CALIBRATION})

# Calculate the compass reading with calibration offset removed and scale applied
cx=$(awk -F' ' "BEGIN {printf(\"%f\",(${mx}-${cx0})/${cscale})}")
cy=$(awk -F' ' "BEGIN {printf(\"%f\",(${my}-${cy0})/${cscale})}")
cz=$(awk -F' ' "BEGIN {printf(\"%f\",(${mz}-${cz0})/${cscale})}")

# Rotate the magnetometer reading according to the direction of gravity
rx=$(awk "BEGIN{ printf(\"%.2f\",${cx}*${R[00]} + ${cy}*${R[01]} + ${cz}*${R[02]} ) }")
ry=$(awk "BEGIN{ printf(\"%.2f\",${cx}*${R[10]} + ${cy}*${R[11]} + ${cz}*${R[12]} ) }")
rz=$(awk "BEGIN{ printf(\"%.2f\",${cx}*${R[20]} + ${cy}*${R[21]} + ${cz}*${R[22]} ) }")

deg=$(awk -F' ' "BEGIN {printf(\"%d\", (180.0/3.14159)*atan2(${ry},${rx}) + 90) }")

# Make the angle range 0 to 360 rather than -180 to 180
if (( $deg < 0 )); then deg=$(($deg + 360)); fi

# Map the angle to cardinal direction
c=$(awk "BEGIN{ printf(\"%d\", 16.0*${deg}/360.0 + 0.5) }")
cardinal=("N" "NNE" "NE" "ENE" "E" "ESE" "SE" "SSE" "S" "SSW" "SW" "WSW" "W" "WNW" "NW" "NNW" "N")

# Display the result
echo "${deg}° ${cardinal[c]}"

Bash:
me@pyra:~/code/bno055$ ./pyra-compass-reset
Compass calibration
   Rotate the pyra in a complex motion
   Including figure of 8s and full rotations.
Press any key to start...

Collected 57 samples
Field strength=5.08uT
Offset=119.22,14.46,127.19

me@pyra:~/code/bno055$ ./pyra-compass
55° NE
 
Last edited:

ouzle

Very Active Member
Joined
Jun 3, 2019
Messages
130
Location
England
For me, the compass is not so reliable when charging. So when you're out in the wilds with the pyra running open street maps, don't forget to unplug the battery pack.
 
  • Like
Reactions: rSl

ouzle

Very Active Member
Joined
Jun 3, 2019
Messages
130
Location
England
@aTc What's the right way to get scripts like these into the pyra repo? Should I create a pyra-bno055 debian package? Is it better to have separate pyra-accelerometer and pyra-compass packages? Are these more fundamental scripts that belong in pyra-scripts?
 

aTc

Very Active Member
Joined
Apr 25, 2009
Messages
225
Not quite sure.
A pyra-sensors package would probably the solution. There are quite a few sensors, and they all seem to need various support scripts.
Although if one of them needs some huge dependencies that might make it a better option to have one package for each sensor.

pyra-scripts does need a huge cleanup :)
 

F_Slim

IDDQD
Joined
Aug 25, 2010
Messages
1,415
Age
41
Location
Germany
I once tried to use the compass in my tablet and it also did not work very well. But it made a huge different if you are indoors or outdoors.
Also there is some kind of calibration which can be done by turning the device around all axis. Is this also true for the chip in the Pyra?
 

hns

Well-Known Member
Joined
Dec 4, 2011
Messages
571
Location
Oberhaching
Handling sensors is quite a standard problem for all handheld devices software stacks. First activities go back to the (OpenMoko) GTA02. But AFAIR there is no "standard" besides Android/Replicant (or iOS). There, all sensors are mapped to some high level abstraction Java/Swift/Obj-C classes which can be queried by any application. All specific things like handling calibration and non-linear effects are hidden from the normal programmer...
This may include calibration matrices, taking magnetic fields by the internal speakers into account and potentially if the device is charged or discharged or a headset or USB device is connected or not.
And, there is a discipline called "sensor fusion" which uses Kalman filters to merge accelerometer/gyroscope data with GPS for more precise attitude reports to applications.
It would be nice if such a "libsensor.so" could be used with a plugin-architecture where specific devices and chips can add their specifics.
Maybe it exists and I am just not aware of it...
 

Kazuki

Kazuki Suzuki Przyborowski · Number 139
Joined
Jul 21, 2010
Messages
27
Age
34
Location
Chicago, Illinois, USA
Website
www.instagram.com
WEBSITE
https://idb.osdn.jp/support/index.php?act=view
LOCATION
Chicago, Illinois, USA
FACEBOOK
kazuki.przyborowski
TWITTER
kazuki2k
YOUTUBE
KazukiPrzyborowski
I once tried to use the compass in my tablet and it also did not work very well. But it made a huge different if you are indoors or outdoors.
Also there is some kind of calibration which can be done by turning the device around all axis. Is this also true for the chip in the Pyra?
Yes you have to tilt and move your device in a figure eight Like this.
All my phones I ever had have always had a bad compass so I had to calibrate it everytime I opened Google maps.
 
Joined
Jul 20, 2021
Messages
1
Hello there.

I've been fiddling around with the Pyra's bno055 driver for the past few weeks, and I wanted to share my findings with the Pyra community. I hope someone finds this information useful.

Intended audience: userspace programmers with basic to intermediate knowledge of Linux

The Pyra's bno055 device driver uses the Linux Industrial I/O (IIO) subsystem. The IIO subsystem provides a standard userspace interface for reading data from devices like accelerometers, ADCs/DACs, or pressure sensors (i.e., devices which can potentially produce anywhere from dozens to millions of samples per second). Linux kernel maintainers recommend that all new device drivers for such devices (or for pretty much any sensor that's intended to be sampled at a rate greater than ~3 Hz) use the IIO subsystem. In principle, any userspace software that provides support for IIO-based acceleration/rotation/magnetic field measurements should automatically support the Pyra's bno055 driver. However, I don't think any such software currently exists (or at least I haven't been able to find any). The only Free userspace software that I know of that supports IIO is GNU Radio (indirectly, via the gr-iio package), but that software is intended for RF devices, not the bno055. If any developers are reading this, please consider adding IIO support to your software.

The Pyra's bno055 driver is currently in an incomplete state, and doesn't fully support all of the bno055's features (more on this later), but it is fairly usable. This driver was originally written by Vlad Dogaru ~5 years ago. It was not integrated into as part of the mainline Linux kernel due to its missing functionality (and because the IIO framework at that time didn't support some of the bno055's features, such as its gravity and linear velocity vectors), but it was integrated into the Letux kernel (the independently-maintained Linux-based kernel that runs on the Pyra).

Breaking news: Just as I was finishing writing this post, I discovered that there has been extensive discussion regarding the bno055 on the Linux kernel mailing list during the past few days; Andrea Merello has proposed a series of patches that add new functionality to the IIO subsystem, and proposed a more extensive, feature-rich bno055 driver. It's very likely that this newer driver will be integrated (or backported) into the Letux kernel at some point, but for now we will have to make do with our current driver.

======== How to interface with the bno055 device driver ========

Since the Pyra's bno055 driver is an IIO driver, we can access its sensor data using any of the following standard interfaces:

The IIO subsystem creates a pseudo file system and represents each of the bno055 driver's userspace-visible inputs and outputs as standalone files. In IIO parlance, each of these inputs and outputs is referred to as an "attribute". Reading from/writing to a file is equivalent to reading from/writing to the corresponding IIO attribute. Be aware that the values read from/written to these files must be formatted as strings, not binary numbers (e.g., the number 10 must be represented as the character sequence "10", not as the binary value of 10).

The bno055 driver's attributes can be found in /sys/bus/iio/devices/iio:device#/(replacing "#" with the bno055's IIO device index). On my Pyra (and probably on yours, too), the bno055 is registered as "iio:device1". However, for the sake of portability and forwards compatibility, it's recommended not to assume that the bno055 is "iio:device1"; rather, as indicated in ouzle's post, it's preferable to determine the bno055's IIO device index programmatically -- by examining every "iio:device#" directory and selecting the one whose "name" property is equal to "bno055". libiio (described further below) allegedly provides utilities to search for specific IIO devices, to avoid the need for boiler-plate device-finding code.

The set of available attributes (i.e., the set of files that appear in the "iio:device#" directory) depends on the bno055's current operation mode (See the section below for more information).

The sysfs interface described above is primarily intended for low-frequency or on-demand polling. For high-frequency polling, the IIO subsystem provides a feature which will automatically sample an IIO device at a user-configurable rate and store the data to a buffer; this buffer will be exposed to userspace as a character device file.

Unfortunately, the bno055's device driver does not currently implement the necessary logic to allow us to use this feature. This isn't too big of a concern for now, since I suspect that the sysfs interface will be more than sufficient for the Pyra's typical use cases (e.g., I don't foresee us needing to sample the accelerometer at the bno055's maximum allowable 1000 Hz rate any time soon).

"libiio" is a userspace library that provides a higher-level abstraction of the interfaces described above. I haven't tried this library myself, so I can't say much about it, but it seems to be the preferred way for userspace software to interface with IIO devices. It apparently provides bindings for several languages (C, C++, Python, C#).

======= Making sense of the bno055 driver's IIO attributes =======

The spoiler further below lists all the IIO attributes currently supported by the bno055 driver, along with a brief description of how to interpret each attribute. Only a subset of these attributes will be available at any given time. The exact subset depends on the bno055's current operation mode. Note that the bno055 driver will select an operation mode of "BNO055_MODE_AMG" by default, which will produce raw accelerometer, magnetometer, and gyroscope readings, but will not produce any "fusion mode" readings (more on this later).

Attributes come in groups, and all the attributes in a given group have names that begin with the same prefix. You'll notice that most of these group contain attributes whose name ends with the suffixes "_raw", "_offset", or "_scale". As noted in ouzle's post, these "_offset" and "_scale" values must be applied to the "_raw" value to determine the attributes' real sensor value. However, ouzle's formula is applying "_offset" and "_scale" in the wrong order; "_offset" must be added before "_scale" is applied. The formula for computing sensor value XXXX is:

XXXX = (XXXX_raw + XXXX_ offset) * XXXX_scale

Be aware that "_offset" attributes will only appear when the bno055's operation mode is not in a "fusion mode". When the bno055 is in "fusion mode", the device's built-in software will implicitly apply any necessary offsets to its raw sensor readings. Thus, the formula for computing sensor value XXXX in "fusion mode" is simply:

XXXX = XXXX_raw * XXXX_scale

name
- This attribute is available in all operation modes. It returns the constant string "bno055"

in_temp_input
- This attribute is available in all operation modes. It returns the bno055's temperature, measured in units of milli degrees Celcius (i.e., to convert to Celcius, divide in_temp_input by 1000).

in_accel_scale
in_accel_x_offset
in_accel_y_offset
in_accel_z_offset
in_accel_x_raw
in_accel_y_raw
in_accel_z_raw
- These attributes are available in all operation modes in which the accelerometer is enabled. Applying *_offset (if applicable) and *_scale to _raw will give you acceleration values, measured in units of m/(s^2).

in_anglvel_scale
in_anglvel_x_offset
in_anglvel_y_offset
in_anglvel_z_offset
in_anglvel_x_raw
in_anglvel_y_raw
in_anglvel_z_raw
- These attributes are available in all operation modes in which the gyroscope is enabled. Applying *_offset (if applicable) and *_scale to _raw will give you angular velocity values, measured in units of radians per second.

in_magn_scale
in_magn_x_offset
in_magn_y_offset
in_magn_z_offset
in_magn_x_raw
in_magn_y_raw
in_magn_z_raw
- These attributes are available in all operation modes in which the magnetometer is enabled. Applying *_offset (if applicable) and *_scale to _raw will give you magnetic field readings, measured in units of Gauss.

in_magn_scale
in_magn_x_offset
in_magn_y_offset
in_magn_z_offset
in_magn_x_raw
in_magn_y_raw
in_magn_z_raw
- These attributes are available in all operation modes in which the magnetometer is enabled. Applying *_offset (if applicable) and *_scale to _raw will give you magnetic field readings, measured in units of Gauss.

in_rot_quaternion_scale
in_rot_quaternion_raw
in_rot_scale
in_rot_x_raw
in_rot_y_raw
in_rot_z_raw
- These attributes are available in all "fusion mode" operation modes. Applying in_rot_scale to in_rot_x/y/z_raw will give you the device's orientation as Euler angles, measured in degrees. Applying in_rot_quaternion_scale to each of in_rot_quaternion_raw's components (w, x, y, and z are presented as a single attribute, and are separated by spaces) will give you the device's orientation as a quaternion.

========= Changing the bno055's settings =======

Currently, the bno055 driver does not provide any way to change the device's settings from userspace. On startup, the driver configures most of the However, the driver does provide a "bosch,operation-mode" property (which is configured in the Pyra's device tree file) that determines what operation mode the driver will boot into. By editing your Pyra's device tree file, changing the value of "bosch,operation-mode" file, and then performing a reset, you can change the bno055's operation mode.

Here's a simple way to edit your Pyra's device tree file, without having to touch any source code:

1) Install the Debian "device-tree-compiler" package.
2) Find out which device tree file your Pyra is using, and find where the file is stored. On my Pyra it's "/boot/dtb/linux-image-5.6.19-daveiii-pyradef-aufs/omap5-letux-cortex15-v5.3+pyra-v5.3.dtb". As of this writing, it's probably the same on your Pyra, too.
3) Determine the value of the "bosch,operation-mode" property by executing the following command:

ftdget /boot/dtb/linux-image-5.6.19-daveiii-pyradef-aufs/omap5-letux-cortex15-v5.3+pyra-v5.3.dtb i2c2/bno055@29 bosch,operation-mode

4) Set the "bosch,operation-mode" property to a specific value by executing the following command (replacing "###" with the numeric value of the operation mode you'd like to select):

sudo ftdput /boot/dtb/linux-image-5.6.19-daveiii-pyradef-aufs/omap5-letux-cortex15-v5.3+pyra-v5.3.dtb i2c2/bno055@29 bosch,operation-mode ###

(Refer to Table 3-5 of the bno055 datasheet document to determine each operation mode's corresponding numeric value. For example, "IMU" mode corresponds to a numeric value of 8).

========= Features that are currently unsupported =======
Buffering; interrupts; automatic calculation of linear velocity and gravity vectors; access to most of the bno055's configuration registers.

It looks like Andrea Merello's new proposed bno055 will fix most of these issues.

========= Suggestions to maximize application portability =========
If you decide to add IIO support to your software, I suggest that you write your software to search for any IIO device that happens to contain the attributes you're interested in (rather than hardcoding your application to assume the presence of a bno055). This will allow your software to work out-of-the box on other systems with different accelerometer/gyroscope/magnetometer sensors.
 

ouzle

Very Active Member
Joined
Jun 3, 2019
Messages
130
Location
England
However, ouzle's formula is applying "_offset" and "_scale" in the wrong order; "_offset" must be added before "_scale" is applied.
I've corrected the order in the OP now. All the offsets are default zero on the pyra so order has no effect.
 
  • Like
Reactions: rSl
Top