Freescale Sensor Fusion Library for Kinetis MCUs

Freescale Sensor Fusion Library for Kinetis MCUs
Hands-On Workshop: Freescale
Sensor Fusion Library for
Kinetis MCUs
AMF-CON-T0756
Michael Stanley | Sensor Solutions Division
S E P T. 2 0 1 4
TM
External Use
Agenda
•
•
hour 1
•
•
•
Part 1: Motion Sensors Overview
Part 2: Movement and Orientation
Part 3: Introduction to Sensor Fusion
Part 4: Freescale Sensor Fusion Toolbox
Part 5: Lab #1 – Play with fusion options
Break
hour 2
•
Part 6: Freescale Sensor Fusion Library
Break
Part 7: Lab #2 – Build the embedded firmware
• Part 8: Optional Lab #3 – Make some changes
• Part 9: Odds & Ends and Wrap-up
•
hour 3
TM
External Use
1
Part 1: Motion Sensors Overview
TM
External Use
2
Sensors are more pervasive than you might think –
Just check your pocket…
TM
External Use
3
Some Sensors are Physical, Some are “Virtual”
Sensor Type
Caveat
Accelerometer
With gravity
Physical
Without gravity
Virtual
Linear Acceleration
Gravity
Physical / Virtual
Virtual
Magnetic Field
Uncalibrated
Physical
Magnetic Field
Calibrated
Virtual
Gyroscope
Uncalibrated
Physical
Gyroscope
Calibrated
Virtual
Orientation
Rotation Matrix
Virtual
Orientation
Azimuth, pitch, roll and
rotation matrix
Virtual
Ambient Temperature
Physical
Light
Physical
Pressure
Physical
Proximity
Physical
Relative Humidity
Physical
Items in red are not supported by Freescale sensors.
TM
External Use
4
Some Sensors are Physical, Some are “Virtual”
Sensor Type
Caveat
Rotation Vector
Physical / Virtual
9-axis
Virtual
Game Rotation Vector
Accel/gyro only
Virtual
Geomagnetic Rotation Vector
Accel/mag only
Virtual
Significant Motion
Virtual
Step Detector
Virtual
Step Counter
Virtual
• The list above summarizes sensors & sensor fusion components that might be
expected components for modern operating systems.
• All but the last 4 listed are supported by Android 4.3. “KitKat” offers support for the
last four.
• Other OS’s continue to evolve in a similar fashion.
• The possible list of sensors and types of sensor fusion is virtually unlimited.
TM
External Use
5
In this course…
•
Because “Sensor Fusion” is an extremely broad topic, this course
focuses on some specific examples:
− Magnetic
calibration
− Electronic compass
− Virtual gyro
− Compute orientation
− Compute linear acceleration sans gravity
•
Sensors used include: Accelerometer + Magnetometer + Gyro
• For today’s session, we are ignoring: vibration analysis, gesture
detection, contextual awareness, navigation / location, auto crash
detection, auto stability control, etc.
TM
External Use
6
Sensor Strengths & Weaknesses
Sensor
Strengths
Weaknesses
Accelerometer
• Inexpensive
• Extremely low power
• Very linear
• Very low noise
• Measures the sum of gravity and
acceleration. We need them
separate.
Magnetometer
• The only sensor that can orient
itself with regard to “North”
• Insensitive to linear acceleration
• Subject to magnetic interference
• Not “spatially constant”
Gyro
• Relatively independent of linear
acceleration
• Can be used to “gyro-compensate”
the magnetometer
• Power hog
• Long startup time
• Zero rate offset drifts over time
Pressure Sensor
• The only stand-alone sensor that
can give an indication of altitude
• Not well understood
• A “relative” measurement
• Subject to many interferences and
environmental factors
TM
External Use
7
An Accelerometer Measures Linear Acceleration plus
Gravity
An accelerometer by itself is a “3 axis” system
When any axis is vertical, we cannot
detect rotation about that axis
+Z
+X
• Accelerometer
+Y
TM
External Use
8
What do we mean: Accelerometers measure linear
acceleration plus gravity?
When horizontal, and at rest:
X=0
Y=0
Z = 1g
TM
External Use
9
When horizontal, and
accelerating at 1g in the direction
of the arrow:
X = 1g
Y=0
Z = 1g
Adding a gyroscope
This “6 axis” system is known as an
Inertial Measurement Unit or “IMU”
This is a Right Hand System (RHR)
+Z
ωz
+X
• Accelerometer
• Gyro
ωx
+Y
ωy
A 3-axis gyroscope measures angular velocity about each of the 3 axes.
TM
External Use
10
Adding a magnetometer
This “9 axis” system is known as a magnetic, angular rate & gravity (MARG) sensor
Add a processor and you have an attitude & heading reference system (AHRS)
+Z = up
ωz
+X = East
• Accelerometer
• Gyro
• Magnetometer
+Y = North
ωy
ωx
ENU
A 3-axis magnetometer gives you the X/Y/Z components of the magnetic field.
TM
External Use
11
As an aside...
horizontal intensity = 23.4T
In Grapevine Texas, during the week of FTF2014, almost 2/3 of
the earth’s magnetic field is directed DOWN
TM
External Use
12
Adding a pressure sensor
This is a “10 axis” system
+Z = up
ωz
+X = East
• Accelerometer
• Gyro
• Magnetometer
• Pressure
+Y = North
ωy
ωx
ENU
Pressure is a scalar (versus vector) quantity
TM
External Use
13
Pressure can give you an estimate of altitude
Altitude = K1 X (1 - (P/P0)K2)
• K1 = 44330.77 meters
• K2 = 0.190263 (unitless)
• P0 = 101325 Pascals
TM
External Use
14
Notice this is a log scale...
Typical Sensor Power in W
100000
14500
9500
10000
Power in W
1437.5
662.5
1000
1100
234
151.2
100
100
20
10
1
12.5Hz LP 400Hz LP
Acc only
Acc
12.5Hz
Mag
400Hz Mag Pedometer Pressure Hybrid Mag
Gyro
(1Hz)
+ Accel
Standby
(200Hz)
This chart was created 2013, you can expect numbers to decrease over time.
TM
External Use
15
Gyro RUN
Gyro Ready to active = 2/ODR + 10ms
Typical Sensor Power in W
16000
14500
14000
• Notice that the magnetometer
currents are approximately 5X to 6X
the accelerometer currents
12000
Power in W
10000
9500
8000
6000
4000
2000
1437.5
20
234
100
662.5
1100
151.2
0
12.5Hz LP 400Hz LP
Acc only
Acc
TM
External Use
16
12.5Hz
Mag
400Hz Mag Pedometer Pressure Hybrid Mag
Gyro
(1Hz)
+ Accel
Standby
(200Hz)
Gyro RUN
Some observations
Accelerometers are the most power efficient motion sensor you’ll
find
They often include motion detection circuits – use those to power
the system up/down for idle periods
Accelerometers are low power because they are usually “passive”
devices. The proof mass moves only when the device is in motion.
Gyros have continuously moving proof masses, requiring much
higher currents to keep them in motion
TMR1-based magnetic sensors are arranged in a Wheatstone
bridge formation – requiring DC biases
Another good sensor to “gate” others is an ambient light sensor
•
•
•
•
•
•
1
TMR = Tunneling MagnetoResistive
TM
External Use
17
Typical “Minimum” Sensor Complements / Application
Application
Acc
Mag
Portrait/landscape, tap detect, fall
detection
X
Pedometry, vibration analysis, tiltmeter
X
eCompass, pointing/remote control,
augmented/virtual reality
X
X
Virtual gyro
X
X
Gyro-compensated eCompass
X
X
Activity monitors
X
X
X
Gyro
X
X
Motion capture
X
X
X
3D mapping & localization
X
X
X
Image stabilization, gesture recognition
X
TM
External Use
18
Pressure
X
X
Part 2: Movement and Orientation
TM
External Use
19
Movement
Any movement from point A to point B can be decomposed into a translation
plus optional rotation
Position and Orientation
4.5
Ψ
4
3.5
3
Z
2.5
2
1.5
1
ΔY
0.5
0
-0.5
5
ΔX
4
4
3
3
2
2
1
1
0
Y
0
-1
-1
X
We need at least 6 degrees of freedom (DOF) to describe a
movement in 3 dimensions: ΔX, ΔY, ΔZ, , θ, Ψ
TM
External Use
20
Frames of Reference
•
Most systems use a Cartesian frame of reference, but which one?
+Y = East
+X = North
+Z = Up
NED
+X = East
+Z = Down
+Y = North
ENU
Aeronautics uses NED
Windows 8 and Android use ENU
TM
External Use
21
There can be multiple, concurrent, frames of reference
Up
Body or Device Reference Frame
East
Earth Frame
North
The device orientation can be defined as the rotation necessary to
map the global frame of reference into alignment with the body
frame of reference (or vice versa).
TM
External Use
22
There are multiple representations for rotation
Options are:
• Euler Angles – intuitive (roll, pitch & yaw), but subject to gimbal lock
• Rotation Matrices – rotation as a matrix multiplication
• Axis / Angle – easy to understand, difficult to use
• Quaternions – similar to axis/angle, with a theoretical background that
makes them useful
• Freescale sensor fusion libraries support all forms
axis/angle
Euler Angle Illustration
source: http://en.wikipedia.org/wiki/File:Euler2a.gif
TM
External Use
23
Part 3: Introduction to Sensor Fusion
TM
External Use
24
What is Sensor Fusion?
Sensor fusion encompasses a variety of
techniques which:
•
Trade off strengths and weaknesses of the various
sensors to compute something more than can be
calculated using the individual sensors;
•
Improve the quality and noise level of computed
results by taking advantage of:
− Known data redundancies between sensors
− Knowledge of system transfer functions, dynamic
behavior and/or expected motion
TM
External Use
25
Sensor Fusion Data Flow for Consumer Devices
Sensor Hub Functions
Configure, Power State, Data Control
Pressure
hi/low/band
pass filtering
Trim
Pressure
Shake detection
shake event
3-Axis Acc
FoR
mapping
Trim
hi/low/band
pass filtering
Acc
3-Axis
Gyro
FoR
mapping
Trim
hi/low/band
pass filtering
ω x,y,z
3-Axis
Mag
FoR
mapping
Trim & Hard/Soft
compensation
hi/low/band
pass filtering
B x,y,z
Raw data calibration
parameters
FoR = Frame of Reference Mapping
Calculate
hard/soft iron
parameters
x,y,z
Rotation matrix
Kalman
Filter or
similar
function
Quaternion
Geometric
computations
Tilt-compensated mag
heading
Orientation (φ, Θ, Ψ)
Sensor Fusion
TM
External Use
26
Magnetic Calibration
Soft Iron in fixed spatial relationship to
the sensor distorts the measured field.
The sphere is distorted into an ellipsoid.
Hard Iron (permanent magnet) in fixed
spatial relationship to the sensor adds an
offset.
Ideal
Measured
Both are linear effects1, and can be reversed – if you know what you are doing!
1
Assuming there is no magnetic hysteresis present
TM
External Use
27
Freescale Magnetic Calibration Library
•
Now bundled into the sensor fusion
library
−
4 and 7 and now 10 element solvers are
available in source form
• As a virtual sensor in Freescale’s Intelligent
Sensing Framework (ISF)
• Freescale’s eCompass software received the
Electronic Products Magazine 2012 Product of
the Year Award.
TM
External Use
28
Magnetic Calibration Variations
Bcx
Bc = W-1(Bp – V)
where:
Bc
W-1
Bp
V
Bcy
Bcz
=
s1
s2
s3
Bpx
-
Vx
s2
s4
s5
Bpy
-
Vy
s3
s5
s6
Bpz
-
Vz
Calibrated magnetic vector
Inverse Soft Iron Matrix
Physical magnetic measurement
Hard Iron Offset Vector
The 4-element calibration computes Vx, Vy and Vz hard iron offsets plus magnitude of the geomagnetic
vector . W-1 = identity matrix
The 7-element calibration also computes s1, s4 and s6. Off diagonal components of W-1 are 0.
W-1 =
s1
0
0
0
s4
0
0
0
s6
The 10-element calibration computes all elements of W -1, including s2, s3, and s5
W-1 =
s1
s2
s3
s2
s4
s5
s3
s5
s6
Everyone uses the same equation.
The magic is in how you compute the coefficients.
TM
External Use
29
Electronic Compass
Once you have performed magnetic calibration, computing magnetic
north is easy using cross products
b
axb
Step 1: Eastest = Bc X A
Step 2: Normalize East = Eastest / |Eastest|
Step 3: Normalize Up = A / |A|
Eastest
Step 4: Magnetic North = A X East
a
A = accelerometer reading
Up
East
Step 1
North
Step 4
B
Bc
See getRotationMatrix function at:
http://developer.android.com/reference/android/hardware/SensorManager.html
TM
External Use
30
|a x b|
Orientation
Orientation can be thought of as a rotation from some standard reference (usually
the global frame).
For a set of sensors at rest, orientation can be considered to be the 3D rotation
necessary to map magnetic north into calibrated magnetic field reading and gravity
to measured accelerometer reading.
B = RM
0
BN
BZ
magnetic north in the ENU
frame of reference. BN is the
horizontal component of the
earth field, BZ is the vertical.
A = RM
A
B
|B|
RM
ENU
External Use
31
gravity in the ENU1
frame of reference
= accelerometer reading (in gravities) at rest
= measured magnetic field after calibration
= magnitude of the earth field
= rotation matrix = orientation
= X=East, Y=North, Z=Up
1
TM
0
0
1
Use [0, 0, 1]T Windows 8. Use [0, 0, -1]T for Android.
Virtual Gyro
If you calculate orientation from accel + mag, computing outputs for a virtual
gyro is easy:
angular rates = the time derivative of orientation
For rotation of fixed reference frame relative to body frame (equivalent to a gyro
output), we have:
1
Small signal rotation matrix = R = RRR = 
-
dR/dT = d(RRR)/dT =
0
ωz
-ωy
-
1

-ωz
0
ωx

-
1
ωy
0 1,2 1,3
-ωx = ( 1/Δt ) ( Rt+1 RtT – I3x3 ) = 2,1 0 2,3
0
3,1 3,2 0
ωx = (2Δt)-1 (3,2 - 2,3)
ωy = (2Δt)-1 (1,3 - 3,1)
ωz = (2Δt)-1 (2,1 - 1,2)
This derivation utilizes small angle approximations. See https://community.freescale.com/community/the-embeddedbeat/blog/2013/03/12/building-a-virtual-gyro for derivation details.
TM
External Use
32
Taking it up a notch
•
•
•
•
•
The MagCal / eCompass example is nice because it can be
explicitly calculated
Other systems can be much more complex
If we can model a system as a set of state variables, then we can
use a Kalman filter to separate noise from desired system behavior
A Kalman filter essentially does a linear regression between
measured and expected system response.
Results can be proved to be optimum in a least-squares sense.
TM
External Use
33
6-Axis Accel + Gyro Indirect Kalman Filter
linear acceleration, orientation error
accelerometer
signal
AccelerometerBased Inclination
Model
+
Kalman Filter
gyro
signal
orientation
gyro offset
magnetic disturbance
Gyro-Based
Inclination Model
gyro offset, orientation error
•
This algorithm has no sense of magnetic north
• The output orientation may drift about the gravity vector as a
result of uncorrected gyro gain errors
TM
External Use
34
9-axis accel + mag + gyro Indirect Kalman Filter
linear acceleration, orientation error
accelerometer
signal
AccelerometerBased Inclination
Model
+
gyro signal
Gyro-Based
Inclination
Model
gyro offset,
orientation error
Kalman Filter
+
magnetometer
signal
MagnetometerBased Inclination
Model
magnetometer disturbance, orientation error
TM
External Use
35
orientation
gyro offset
magnetic disturbance
linear acceleration
Part 4: Freescale Sensor Fusion Toolbox
TM
External Use
36
Computing information is only
half the puzzle.
You have to do something with it.
Enter…
TM
External Use
37
The Freescale Sensor Fusion Toolbox
Accel
=
Embedded board running
the Freescale Sensor
Fusion Library for Kinetis
Gyro
Mag
Kinetis MCU
Bluetooth Module
Sensor output data is “fused” using
Freescale-developed code running on
Kinetis, and then “beamed” to a PC or
Android device, where it drives the GUI
Phone/tablet running the Freescale
Sensor Fusion Toolbox for Android
TM
External Use
38
The Freescale Sensor Fusion Toolbox
•
Provides visualization functions for the fusion library
•
Allows you to experiment with different sensor/algorithm choices
•
Gives you access to raw sensor data
•
Allows you to log sensor and fusion data for later use
•
Works with demo and development versions of the Freescale Sensor
Fusion Library
•
Platforms
• Android
• Windows PC
TM
External Use
39
The Freescale Sensor Fusion Toolbox Features by Platform
Feature
Android
PC
Bluetooth wireless link

Requires BT on PC (built-in or dongle)
Ethernet wireless link
On WiGo board only
-
UART over USB
-
1
OS requirements
>= Android 3.0
>= Windows 7.0
Support for native sensors

-
Device View


Panorama View

-
Statistics View

-
Canvas View

-
Orientation XY Plots
-

Inertial XY Plots
-

Magnetics
-

Kalman
-

Altimeter XY Plots
-

Data Logging Capability


Integrated documentation


Availability
Google Play
Freescale website
Price
Free
Free
1 FRDM_K64F
TM
and FRDM_K20D50M projects require a Processor Expert configuration change to
run in wired mode.
External Use
40
PC Version – Device View
1.
2
2.
3
3.
Rotating 3D PCB display
Image align function
Navigation Tabs for:
•
4
Sensors Data Tab
Dynamics Tab
Magnetics
Kalman
Altimeter
Help
•
•
1
•
5
•
•
6
4.
Packet information
•
•
7
•
5.
Figures are from 28 August 2014 build of the application.
Appearance may vary for other versions.
6.
7.
choice of PC comm port
packet activity indicator
# of packet errors
Roll/Pitch/Yaw & MagCal status
Choice of sensor set & algorithm
Sensor board run time and build
parameters, Data logging on/off
This is the most intuitive way to confirm that your sensor fusion is working properly.
TM
External Use
41
PC Version – Sensors Tab
1
Raw Accelerometer Values
2. Calibrated Magnetometer
Values
3. Raw Gyroscope Values
1.
2
3
The PC is used for display only. All values are computed on the embedded board.
TM
External Use
42
PC Version – Dynamics Tab
1
Roll, pitch & compass heading
2. Current quaternion
3. Angular velocity
4. Linear Acceleration
1.
2
3
4
The PC is used for display only. All values are computed on the embedded board.
TM
External Use
43
PC Version – MagneticsTab
2
3
6
1.
2D representation of the
data point “cloud” used
for hard/soft iron
compensation
2.
Computed hard iron
vector
3.
Soft iron matrix
4.
Statistics
5.
Calibration status light
6.
Save to text file
4
6
1
1
1
You can use this display to view how the magnetic constellation evolves
over time in response to changing magnetic environments.
TM
External Use
44
PC Version – Kalman Tab
Use this tab to view
how well your sensor
fusion “digests”
changes in its
environment.
1
2
3
1. Error in orientation estimate (X,Y,Z)
2. Computed gyro offset
3. Error in gyro offset estimate (X,Y,Z)
TM
External Use
45
PC Version – Altimeter Tab
1.
Altitude
2.
Temperature
Not available when using FRDM-FXS-9AXIS board
TM
External Use
46
PC Version – Help tab
TM
External Use
47
Important Point
•
The template programs contained in the Freescale Sensor Fusion
Library for Kinetis MCUs assume that you are utilizing the FRDMFXS-MULTI-B Bluetooth board.
• KL25Z, KL26Z and KL46Z projects can also be used via
UART/USB wired interface by the simple expedient of removing
jumper J7, which powers the Bluetooth module.
• This works because the same UART is drives the Bluetooth module
and the OpenSDA UART interface.
•
K20D50M and K64F use separate physical UARTS for Bluetooth
and OpenSDA. You will need to reconfigure the Processor Expert
UART component in these projects if you wish to use a wired
UART/USB interface. Additional detail is in the user manual.
TM
External Use
48
Android Version Program Operation
output selection
sensor selection
algorithm
selection
Panorama View
accel only
Sensors
Native to your
Android
Device
mag (2d)
Device View
gyro only
Remote
Sensor
Interface
accel/mag
accel/gyro
Log Window
9-axis
Statistics View
Canvas View
Not Shown
TM
External Use
49
ENU Frame of
Reference
+Y
X = East
Y = North
Z = up
+X
+Z is out of page
TM
External Use
50
Application Controls
Navigation
Control
Android Action Bar
The Options
Menu is a button
on some devices
Fusion Settings Bar
Sensor & Algorithm
Selection Here
TM
External Use
51
If present, click to enable
low pass filter
Stats Page
For mag / accel / gyro and
rotation, the “Statistics” Views
displays:
• sensor description
• current sensor value
• min / mean / max values
• standard deviation
• noise / √Hz
When used with the “local” sensor
sources, this is a great way to gain
insight into devices from the
competition!
TM
External Use
52
If you would like to try it…
http://play.google.com/store/apps/details?id=com.freescale.sensors.sfusion
TM
External Use
53
Part 5: Hands on #1 – Play with fusion
options
TM
External Use
54
For this lab
You need
• Freescale Freedom boards shown
• USB cable
• Freescale Sensor Fusion Toolbox running
on a Windows Laptop
FRDM-FXS-9AXIS or
FRDM-FXS-MULTI or
FRDM-FXS-MULTI-B
FRDM-KL25Z or
FRDM-KL26Z or
FRDM-KL46Z or
(C:\Program Files\Freescale\Freescale Sensor Fusion
Toolbox/SensorFusion.exe)
•
Freescale Sensor Fusion Library for
Kinetis MCUs
Make sure the switch on the top sensor
board is “on”.
If you have a MULTI-B board, remove
jumper J7
TM
External Use
55
Plug your USB cable in
this connector
Experiment with each of the following options
#
Option
Comments
1
Accelerometer
Roll & Pitch only, no yaw
2
Gyroscope
Roll, Pitch & Yaw, but no absolute reference
3
Accelerometer +
Magnetometer (eCompass)
Roll, Pitch & Yaw relative to earth frame, but sensitive to
magnetic interference and linear acceleration
4
Accelerometer + Gyroscope
Roll, Pitch with respect to horizontal plane, yaw is relative
5
9-Axis Accelerometer +
Gyroscope + Magnetometer
Roll, Pitch & Yaw relative to earth frame, relatively independent
of magnetic interference and linear acceleration
Experiment with each tab function on the fusion toolbox
TM
External Use
56
Part 6: Freescale Sensor Fusion Library for
Kinetis
TM
External Use
57
Freescale Sensor Fusion Library for Kinetis MCUs
•
Optimized for the computation of orientation with respect to a global
frame of reference as a function of sensor readings from:
−
−
−

accelerometer
and/or gyroscope
and/or magnetometer
Along with orientation, also computes:
−
−
−
−
−
−
linear acceleration
magnetic interference and correction factors for same
magnetic inclination angle
gyroscope zero-rate offset
compass heading
virtual gyro from accelerometer / magnetometer
TM
External Use
58
Freescale Sensor Fusion Library for Kinetis MCUs
•
•
•
•
•
•
Supplied under open source BSD license from Freescale
Implemented as pure C-code sitting on top of device driver and
MQX-lite implementations created via Processor Expert
Shipped in the form of CodeWarrior projects compatible with the
Freescale Sensor Fusion Toolbox
Downloadable from http://www.freescale.com/sensorfusion
Community support available at
https://community.freescale.com/community/sensors/sensorfusion
Contract support services offered by Freescale. Contact:
[email protected] for details.
TM
External Use
59
FREESCALE END-USER SOFTWARE LICENSE AGREEMENT
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following
conditions are met:

Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.

Neither the name of Freescale Semiconductor, Inc. nor the names of its contributors may be used to endorse or promote
products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR,
INC. BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
TM
External Use
60
Freescale Sensor Expansion Boards
Kinetis KL25Z and K20D50M compatible Freescale Sensor Expansion Boards
Part Number
FRDM-FXS-
FRDM-FXS-MULTI*
KL25Z or
KL26Z or
KL46Z or
K20D50M or
K64F
FRDM-FXS-MULTI-B*
FRDM-FXS-9AXIS*
TM
External Use
61
Description
Freescale Sensor
Expansion board
MPL3115A2
MMA8652
FXAS21000
FXOS8700
FXLS8471
MMA955x
MAG3110
Freescale Sensor
Expansion board with
Bluetooth and Battery
MPL3115A2
MMA8652
FXAS21000
FXOS8700
FXLS8471
MMA955x
MAG3110
Freescale Sensor
Expansion board with only
2 sensors
FXAS21000
FXOS8700
Pricing
Availability
$50
Now
$125
Now
$30
Now
Ordering Details
Component
Price
Location
Sensor Fusion Library for Kinetis MCUs
Free
http://www.freescale.com/sensorfusion
Freescale Freedom Development Platform
KL25Z = $12.95
KL26Z = $15.00
KL46Z = $15.00
K20D50M = $18.00
K64F = $29.00
http://www.freescale.com/freedom
Freescale Freedom Development Platform
for Multiple Freescale Sensors
$30
$50
$125
http:www.freescale.com/FRDM-FXS-9AXIS
http:www.freescale.com/FRDM-FXS-MULTI
http:www.freescale.com/FRDM-FXS-MULTI-B
Freescale Sensor Fusion Toolboxes For
PC
Free
http://www.freescale.com/sensorfusion
Freescale Sensor Fusion Toolboxes
Android
Free
https://play.google.com/store/apps/details?id=com.freescale
.sensors.sfusion
Freescale Sensors
Various
http://www.freescale.com/sensors
Prices are current as of 6 Sept, 2014. They may vary in the future.
TM
External Use
62
Features vs. Sensor Set
Feature
Accel only
Accel + gyro
Accel + mag
Accel + mag + gyro
Low Pass
Indirect Kalman
Low Pass
Indirect Kalman
Roll / Pitch / Tilt in degrees
Yes
Yes
Yes
Yes
Yaw in degrees
No
No
Yes
Yes
virtual 2 axis2
Yes
virtual 3 axis
Yes
Compass heading (magnetic north) in degrees
No
No
Yes
Yes
Quaternion and rotation vector
Yes
Yes
Yes
Yes
Rotation matrix
Yes
Yes
Yes
Yes
Linear acceleration separate from gravity
No
Yes
No
Yes
NED (North-East-Down Frame of Reference
Yes3
Yes3
Yes
Yes
ENU (Windows 8 variant) Frame of Reference
Yes3
Yes3
Yes
Yes
ENU (Android variant) Frame of Reference
Yes3
Yes3
Yes
Yes
Magnetic calibration included
No
No
Yes
Yes
Gyro offset calibration included
N/A
Yes
N/A
Yes
FRDM-KL25Z board support
Yes
Yes
Yes
Yes
FRDM-KL26Z board support
Yes
Yes
Yes
Yes
FRDM-KL46Z board support
Yes
Yes
Yes
Yes
FRDM-K20D50M board support
Yes
Yes
Yes
Yes
FRDM-K64F board support
Yes
Yes
Yes
Yes
Filter Type
Angular Rate1 in degrees/second
1. Angular rate for configurations with a gyro include corrections for gyro offset
2. Subject to well-known limitation of being blind to rotation about axes aligned with gravity
3. These solutions do not include a magnetometer, therefore there is no sense of compass heading
TM
External Use
63
Option Details
Feature
Details
License
Open Source BSD (see license file for details)
The ANSI C99 source code was optimized on Freescale Kinetis MCUs
based upon ARM Cortex M0+, M4 and M4F processors, but should be
portable to any CPU.
CPU selection
Board customizable
Yes1
Sensor sample rate
Programmable
Fusion rate
Programmable, typically = sample rate/N
Frame of Reference
Programmable (NED, Android, or Windows 8)
Algorithms Executing
Any combination of those shown in the prior slide
Sleep mode enabled between samples/calculations
RTOS
Programmable
MQX-Lite
Code flexibility
All code is supplied in source form
Access to Processor Expert
Yes
* Datasheet, User guide, Application Notes
* Template CodeWarrior projects
* Pre-compiled s-record files
Product Deliverables
FRDM_KL25Z, KL26Z, KL46Z, K20D50M and K64F are supported “out of the box” and may be used as templates for other board/MCU
combinations..
[1]
TM
External Use
64
Sensor Fusion Library Details
TM
External Use
65
Development Requirements
•
You must have either Kinetis Design Studio 1.1.1 or CodeWarrior 10.6
and Processor Expert to build sensor fusion applications using the
Freescale project templates.
−
CodeWarrior can be downloaded from http://www.freescale.com/codewarrior.
− Kinetis Design Studio can be downloaded from http://www.freescale.com/kds
•
•
•
In order to experiment with the demo program, you will need an Android
3.0 or higher device running the Freescale Sensor Fusion Toolbox OR the
PC-based variant of the toolbox. Details are available at
http://www.freescale.com/sensorfusion
Fusion libraries and example projects supplied by the Freescale Sensor
Solutions Division
Development board(s)1 with:
−
Kinetis Cortex-M0+, M4 or M4F MCU
− Freescale FXOS8700CQ 3-axis magnetometer + 3 axis accelerometer
− Freescale FXAS21000 3-axis gyroscope
1
See details on “Freescale Sensor Expansion Boards”. Additional sensor combinations are supported in build.h. And of course,
you can add your own! Future expansion boards may replace the FXAS21000 with the FXAS21002, which is also supported.
TM
External Use
66
Easy to use…
•
Pre-built templates are targeted at specific Freedom boards
• User code easily added to a single .c file within any of the
following functions:
•
−
void UserStartup(void);
−
void UserHighFrequencyTaskInit(void) ; // runs once, the first time through the 200Hz task
−
void UserHighFrequencyTaskRun(void); // runs each time the 200Hz task runs
−
void UserMediumFrequencyTaskInit(void); // runs once, the first time through the 25Hz task
−
void UserMediumFrequencyTaskRun(void); // runs each time the 25Hz task runs
Sensor and fusion values are simply read from predefined
global structures
TM
External Use
67
user_tasks.c Template Page 1 of 3
#include "Cpu.h"
#include "Events.h"
#include "mqx_tasks.h"
#include "UART.h"
#include "include_all.h"
void UserStartup(void) {
// The following UART function call initializes Bluetooth communications used by the
// Freescale Sensor Fusion Toolbox. If the developer is not using the toolbox,
// these can be removed.
//
// initialize BlueRadios Bluetooth module
BlueRadios_Init(UART2_DeviceData);
// put code here to be executed at the end of the RTOS startup sequence.
//
// PUT YOUR CODE HERE
//
return;
TM
External Use
68
user_tasks.c Template Page 2 of 3
void UserHighFrequencyTaskInit(void) {
// User code to be executed ONE TIME the first time the high frequency task is run.
//
// PUT YOUR CODE HERE
//
return;
}
void UserMediumFrequencyTaskInit(void) {
// User code to be executed ONE TIME the first time the medium frequency task is run
//
// PUT YOUR CODE HERE
//
return;
}
void UserHighFrequencyTaskRun(void) {
// The default frequency at which this code runs is 200Hz.
// This code runs after sensors are sampled.
// In general, try to keep "high intensity" code out of UserHighFrequencyTaskRun.
// The high frequency task also has highest priority.
//
// PUT YOUR CODE HERE
//
return;
}
TM
External Use
69
user_tasks.c Template Page 3 of 3
void UserMediumFrequencyTaskRun(void) {
// This code runs after the Kalman filter loop
// The default frequency at which this code runs is 25Hz.
// The following UART function constructs and sends Bluetooth packets used by the
// Freescale Sensor Fusion Toolbox. If the developer is not using the toolbox,
// it can be removed.
// transmit orientation over the radio link
CreateAndSendBluetoothPacketsViaUART(UART2_DeviceData);
//
// PUT YOUR CODE HERE
//
return;
}
Steps to use:
1. Import project into CodeWarrior
2. Add your code as shown above
3. Build
4. Download and run
TM
External Use
70
Access Fusion Inputs & Outputs Via a Standard
Set of Global Data Structures
Input Global Data Structures defined in build.h
Pointer Function
Structure Name
Structure Type
Accelerometer
thisAccel
AccelSensor
Magnetometer
thisMag
MagSensor
Gyroscope
thisGyro
GyroSensor
Output Global Data Structures defined in tasks.h
Pointer Function
Structure Name
Structure Type
Altimeter results
thisSV_1DOF_P_BASIC
SV_1DOF_P_BASIC
3-axis Accelerometer results
thisSV_3DOF_G_BASIC
SV_3DOF_G_BASIC
2D Magnetic-only eCompass results
thisSV_3DOF_B_BASIC
SV_3DOF_B_BASIC
Gyro-only orientation
thisSV_3DOF_Y_BASIC
SV_3DOF_Y_BASIC
eCompass results
thisSV_6DOF_GB_BASIC
SV_6DOF_GB_BASIC
accel+gyro results
thisSV_6DOF_GY_KALMAN
SV_6DOF_GY_KALMAN
9-axis results
thisSV_9DOF_GBY_KALMAN
SV_9DOF_GBY_KALMAN
TM
External Use
71
Location of Variables Within the Global Structures
Description
Data Type
roll in degrees
pitch in degrees
yaw in degrees
compass heading in degrees
tilt angle in degrees
magnetic inclination angle in
degrees
geomagnetic vector
(microTeslas, global frame)
gyro offset in degrees/sec
linear acceleration in the
sensor frame in gravities
linear acceleration in the global
frame in gravities
quaternion (unitless)
Fusion Algorithm Options
float
float
float
float
float
float
G
(accel)
fLPPhi
fLPThe
fLPPsi
fLPRho
fLPChi
N/A
float
N/A
GB
(eCompass)
fLPPhi
fLPThe
fLPPsi
fLPRho
fLPChi
fDelta
fLPDelta
N/A
float
float
N/A
N/A
N/A
N/A
fbPl[3]
faSePl[3]
fbPl[3]
faSePl[3]
float
N/A
N/A
N/A
faGlPl[3]
fq
fLPq
fOmega[3]1
fR[3][3]
fLPR[3][3]
fLPRVec[3]
fdeltat
fq
fLPq
fOmega[3]
fR[3][3]
fLPR[3][3]
fLPRvec[3]
fdeltat
fqPl
fqPl
fOmega[3]2
fRPl[3][3]
fOmega[3] 2
fRPl[3][3]
fRVecPl[3]
fdeltat
fRVecPl[3]
fdeltat
fquaternion
angular velocity in dps
orientation matrix (unitless)
float
float
rotation vector
time interval in seconds
float
float
GY
(accel + gyro)
fPhiPl
fThePl
fPsiPl
fRhoPl
fChiPl
N/A
GBY
9-axis
fPhiPl
fThePl
fPsiPl
fRhoPl
fChiPl
fDeltaPl
N/A
fmGl[3]
Data elements for altimeter, 2D eCompass, and gyro only are not shown.
TM
External Use
72
Here is an Example of Grabbing Quaternion Values
struct fquaternion fq;
// quaternion
float q0, q1, q2, q3;
//fq = thisSV_3DOF_G_BASIC.fLPq; // OR
//fq = thisSV_6DOF_GB_BASIC.fLPq; // OR
//fq = thisSV_6DOF_GY_KALMAN.fqPl; // OR
fq = thisSV_9DOF_GBY_KALMAN.fqPl;
q0 = fq.q0;
q1 = fq.q1;
q2 = fq.q2;
q3 = fq.q3;
// more details/examples are presented in the following section
TM
External Use
73
Example: Reading Euler Angles
Using 3-axis model:
float roll = thisSV_3DOF_G_BASIC.fLPPhi;
float pitch = thisSV_3DOF_G_BASIC.fLPThe;
float yaw = thisSV_3DOF_G_BASIC.fLPPsi;
Using 6-axis accel + mag (eCompass) model:
float roll = thisSV_6DOF_GB_BASIC.fLPPhi;
float pitch = thisSV_6DOF_GB_BASIC.fLPThe;
float yaw = thisSV_6DOF_GB_BASIC.fLPPsi;
Using 6-axis accel + gyro Kalman filter model:
float roll = thisSV_6DOF_GY_KALMAN.fPhiPl;
float pitch = thisSV_6DOF_GY_KALMAN.fThePl;
float yaw = thisSV_6DOF_GY_KALMAN.fPsiPl;
Using 9-axis Kalman filter model:
float roll = thisSV_9DOF_GBY_KALMAN.fPhiPl;
float pitch = thisSV_9DOF_GBY_KALMAN.fThePl;
float yaw = thisSV_9DOF_GBY_KALMAN.fPsiPl;
TM
External Use
74
The Development Kit provides:
•
Access to raw fusion and magnetic calibration functions
• Control over sampling and fusion rates
• Ability to add custom Hardware Abstraction Layer (HAL)
• Access to MQX-Lite customization via Processor Expert
TM
External Use
75
Product Development Kit Structure
As shipped:
• FSFK_KL25Z; or
• FSFK_KL26Z; or
• FSFK_KL46Z; or
• FSFK_K20D50M; or
• FSFK_K64F
<ProjName>
MQXLITE
FLASH
Sources
approximations.h
build.h
drivers.h
Events.h
fusion.h
include_all.h
magnetic.h
<ProjName>.elf
(generated>
include
kernel
psp
Library
prepopulated
with .s19 and
.bin files
matrix.h
mqx_tasks.h
orientation.h
tasks.h
user_tasks.h
approximations.c
drivers.c
Events.c
fusion.c
ProcessorExpert.pe
.project
magnetic.c
main.c
matrix.c
mqx_tasks.c
orientation.h
tasks.c
user_tasks.c
Files in bold red are most likely to
be customized on a per project
basis.
TM
External Use
76
Source File Descriptions
Files
Description
approximations.c
approximations.h
Reduced accuracy/power trig functions
build.h
Build options consolidated into a single file
drivers.c
drivers.h
Initialization of hardware timers and I2C drivers for inertial and
magnetic sensors. Contains
CreateAndSendBluetoothPacketsViaUART().
Events.c
Events.h
Callback functions for hardware events. Contains
UART_OnBlockReceived()
fusion.c
fusion.h
This is where the primary sensor fusion routines reside. All 3, 6 and 9-axis
fusion routines are here.
include_all.h
A catchall for all the other .h files
magnetic.c
magnetic.h
Magnetic calibration functions
TM
External Use
77
Source File Descriptions
Files
Description
main.c
Initializes and executes MQX
matrix.c
matrix.h
Optimized matrix manipulation functions
mqx_tasks.c
mqx_tasks.h
Creates and runs the Sampling, Fusion and Calibration tasks which in
turn call functions in tasks.c
orientation.c
orientation.h
This file contains functions designed to operate on, or compute,
orientations. These may be in rotation matrix form, quaternion form, or
Euler angles. It also includes functions designed to operate with specific
reference frames (Android, Windows 8, NED).
tasks.c
tasks.h
tasks.c provides the high level fusion library interface. It also
includes the option to apply a Hardware Abstraction Layer (HAL).
With proper attention to sensor orientations during PCB design,
tasks.c may never need modification.
user_tasks.c
user_tasks.h
Placeholder functions for // Put your code here
TM
External Use
78
High Level Architecture
Main_task()
RdSensData_task()
Fusion_task()
MagCal_task()
MQX
mqx_tasks
tasks
magnetic
fusion
orientation, matrix,
approximations
drivers & Events
I2C Interface
UART
Interface
GPIO
Interface
user
tasks
RdSensData_Init()
Fusion_Init()
RdSensData_Run()
Fusion_Run()
MagCal_Run()
ApplyAccelHAL()
ApplyMagHAL()
ApplyGyroHAL()
Hardware
generated by
Processor Expert
I2C and UART communications to external devices
are encapsulated by drivers.c and Events.c
primary fusion &
calibration functions
TM
External Use
79
Our Sensor Fusion is Partitioned Into 3 Tasks
200 Hz MQX Hardware Timer
FXOS8700
(Internal clock)
I2 C
Sampling Task
FXAS21000
Specific to hardware
and sensors
25 Hz Software Event
(Internal clock)
Fusion Task
~1 per minute Software Event
Magnetic Calibration
Task
sampling interval = 5 ms
TM
External Use
80
Independent of
hardware and
sensors
The Build Process
Make any desired
changes to the template
run
Processor
Expert
boardspecific
template
For K64, there is one intermediate (and
temporary) step here. Manually edit
CPU_Config.h, change the value for
NV_FSEC to 0xFE. This works around a
bug in the MBED bootloader firmware.
TM
External Use
81
updated
project
with MQXlite
Build
using
Code
Warrior
Test via Freescale
Sensor Fusion Toolbox
for Windows or Android
MCU Resources Used by the Template Projects
Function
FSFK_KL25Z
FSFK_KL26Z
FSFK_KL46Z
FSFK_
K20D50M
FSFK_K64F
Cpu
MKL25Z128VLK4
MKL26Z128VLH4
MKL46Z256VMC4
MK20DX128VLH5
MK64FN1M0VLL12
LED_RED
PTB18
PTE29
PTE29
PTC3
PTB22
Illuminated when a magnetic
calibration is in progress
LED_GREEN
PTB19
PTE31
PTD5
PTD4
PTE26
Flickers when fusion algorithms
are running
LED_BLUE
PTD1
PTD5
PTE31
PTA2
PTB21
Currently unused
FTM
LPTMR0
LPTMR0
LPTMR0
LPTMR0
LPTMR0
Low frequency timer drives the
200 Hz sensor read process
UART
UART0 on
PTA2:1
UART0 on
PTA2:1
UART0 on
PTA2:1
UART1 on
PTE1:0
UART3 on
PTC17:16
Used for Bluetooth
communications
I2C
I2C1on PTC2:1
I2C1 on PTC2:1
I2C1 on PTC2:1
I2C0 on PTB1:0
I2C1 on
PTC11:10
Communicates to sensors
TestPin_KF_Time
PTC10
PTC10
PTC10
PTC10
PTC7
Output lines used for debug
purposes
TestPin_MagCal_
Time
PTC11
PTC11
PTC11
PTC1
PTC5
TM
External Use
82
Description
Fusion Options Are Controlled Via build.h
#ifndef BUILD_H
#define BUILD_H
// PCB HAL options
#define BOARD_WIN8_REV05
0
// with sensor shield
#define BOARD_FRDM_KL25Z
1
// with sensor shield
#define BOARD_FRDM_K20D50M
2
// with sensor shield
#define BOARD_FXLC95000CL
3
#define BOARD_FRDM_KL26Z
4
// with sensor shield
#define BOARD_FRDM_K64F
5
// with sensor shield
#define BOARD_FRDM_KL16Z
6
// with sensor shield
#define BOARD_FRDM_KL46Z
7
// with sensor shield
#define BOARD_FRDM_KL46Z_STANDALONE
8
// without sensor shield
// enter new PCBs here with incrementing values
// C Compiler Preprocessor define in the CodeWarrior project will choose which board to use
#ifdef REV05
#define THIS_BOARD_ID
BOARD_WIN8_REV05
#endif
#ifdef KL25Z
#define THIS_BOARD_ID
BOARD_FRDM_KL25Z
#endif
TM
External Use
83
Fusion Options Are Controlled Via build.h
#ifdef K20D50M
#define THIS_BOARD_ID
#endif
#ifdef FXLC95000CL
#define THIS_BOARD_ID
#endif
#ifdef KL26Z
#define THIS_BOARD_ID
#endif
#ifdef K64F
#define THIS_BOARD_ID
#endif
#ifdef KL16Z
#define THIS_BOARD_ID
#endif
#ifdef KL46Z
#define THIS_BOARD_ID
#endif
#ifdef KL46Z_STANDALONE
#define THIS_BOARD_ID
#endif
// coordinate system for the build
#define NED 0
#define ANDROID 1
#define WIN8 2
#define THISCOORDSYSTEM ANDROID
TM
External Use
84
BOARD_FRDM_K20D50M
BOARD_FRDM_FXLC95000CL
BOARD_FRDM_KL26Z
BOARD_FRDM_K64F
BOARD_FRDM_KL16Z
BOARD_FRDM_KL46Z
BOARD_FRDM_KL46Z_STANDALONE
//
//
//
//
identifier for
identifier for
identifier for
the coordinate
NED angle output
Android angle output
Windows 8 angle output
system to be used
Fusion Options Are Controlled Via build.h
// sensors to be enabled: compile errors will warn if the sensors are not compatible with the algorithms.
// avoid enabling FXOS8700 plus MMA8652 and MAG3110 which will result in sensor read from all sensors
// with the data read first from FXOS8700 and then over-written by data from MMA8652 and MAG3110.
// it will still work but it's a waste of clock cycles.
#define USE_MPL3115
#define USE_FXOS8700
#define USE_FXAS21000
//#define USE_FXAS21002
//#define USE_MMA8652
//#define USE_MAG3110
// enforce a fatal compilation error if the K20D50M board is used with MMA8652
#if (THIS_BOARD_ID == BOARD_FRDM_K20D50M) && defined USE_MMA8652
#error This build creates an I2C conflict between MMA8451 on K20D50M board and MMA8652 on sensor board
#endif
. . .
// normally all enabled: degrees of freedom algorithms to be executed
#define COMPUTE_1DOF_P_BASIC
// 1DOF pressure (altitude) and temperature: (1x pressure)
#define COMPUTE_3DOF_G_BASIC
// 3DOF accel tilt: (1x accel)
#define COMPUTE_3DOF_B_BASIC
// 3DOF mag eCompass (vehicle): (1x mag)
#define COMPUTE_3DOF_Y_BASIC
// 3DOF gyro integration: (1x gyro)
#define COMPUTE_6DOF_GB_BASIC
// 6DOF accel and mag eCompass: (1x accel + 1x mag)
#define COMPUTE_6DOF_GY_KALMAN
// 6DOF accel and gyro (Kalman): (1x accel + 1x gyro)
#define COMPUTE_9DOF_GBY_KALMAN
// 9DOF accel, mag and gyro (Kalman): (1x accel + 1x mag + 1x gyro)
TM
External Use
85
Fusion Options Are Controlled Via build.h
// int16 build number sent in Bluetooth debug packet
#define THISBUILD 420
// sampling rate and kalman filter timing
#define FTM_INCLK_HZ
1000000 // int32: 1MHz FTM timer frequency set in PE: do not change
#define SENSORFS
200
// int32: 200Hz: frequency (Hz) of sensor sampling process
#define OVERSAMPLE_RATIO
8
// int32: 8x: 3DOF, 6DOF, 9DOF run at SENSORFS / OVERSAMPLE_RATIO Hz
// power saving deep sleep
//#define DEEPSLEEP
// define to enable deep sleep power saving
// UART (Bluetooth) serial port control
//#define UART_OFF
// define to measure MCU+algorithm current only
TM
External Use
86
Part 7: Explore the Sensor Fusion Library
TM
External Use
87
For this lab
You need:
• Freescale Freedom boards shown
• USB cable
• Freescale Sensor Fusion Toolbox running on a
Windows Laptop
FRDM-FXS-9AXIS
(C:\Program Files\Freescale\Freescale Sensor Fusion Toolbox/SensorFusion.exe)
•
FSFK_KL25Z project template
KL25Z
(pre-installed on FTF laptops at C:/Temp)
You will install updated software images on your
board.
Make sure the KL25Z switch is “on”
Note: The same process described here works for
any of the fusion library template projects. You can
use any of KL25Z, KL26Z, KL46Z, K20D50M and
K64F Freedom boards.
TM
External Use
88
Plug your USB cable in
this connector
IF your PC has the template pre-installed…
•
SKIP to Step 8
•
Otherwise, repeat Steps 1 through 7 on the following pages
TM
External Use
89
Installation Step 1
a. Copy installer into your working directory
b. Double-click FreescaleSensorFusionInstaller.exe
TM
External Use
90
Installation Step 2
Read the license terms, click “I Agree”
TM
External Use
91
Installation Step 3
a. Review the system requirements.
b. Click “Next”
TM
External Use
92
Installation Step 4
a. Select the destination folder (automatically defaults to the folder in which you
placed the installer).
b. Click “Next”
TM
External Use
93
Installation Step 5
a. Select your choice of kits (defaults to CodeWarrior Fusion Projects and
documentation).
b. Click “Install”
TM
External Use
94
Installation Step 6
a. Click “Close” to complete installation
TM
External Use
95
Installation Step 7
a. Confirm presence of project template, tools and docs directory
TM
External Use
96
Installation Step 8
a. Expand the template folder down into the FSFK_KL25Z/Sources directory
b. Confirm the that the set of files shown below is present
TM
External Use
97
Installation Step 9
a. Start CodeWarrior 10.6
b. Select c:/Temp (or whatever directory you used) as your workspace
c. Click “OK”
TM
External Use
98
Installation Step 10
a. From CodeWarrior, Select
File->Import->General>Existing Projects into
Workspace
b. Click “Next
TM
External Use
99
Installation Step 11
a. Select the proper root
directory
b. Check the project to be
imported
c. Click Finish
TM
External Use
100
Installation Step 12
a. Close the CodeWarrior “Welcome
Screen” if present
b. Expand the project folder to view
contents
Your project has been
successfully installed.
TM
External Use
101
Lab 2, Step 1
a. Double-click on
ProcessorExpert.pe. This will
bring up the components
browser
b. Click on “Generate
Processor Expert Code” icon
to run Processor Expert
TM
External Use
102
Lab 2, Step 2
a. Select the project name
b. Click on the “Build” icon
TM
External Use
103
Lab 2, Step 3
a) Plug your board in if it is not
plugged in
b) Run->Run Configurations
c) Expand CodeWarrior>FSFKKL25Z_FLASH_OpenSDA
(run configuration name
may vary)
d) Click “Run”
TM
External Use
104
Status check
•
You should see the green LED blinking steadily, with a red flash a couple
of times per second
•
You have just successfully reprogrammed your board with the same
application we’ve already experimented with
•
Open up the Freescale Sensor Fusion Toolbox on your PC and confirm
that operation is unchanged
•
Open Sources/drivers.c and review function
CreateAndSendBluetoothPacketsViaUART(). This function pulls virtually
all fusion results from fusion output structures for transmission back to the
Sensor Fusion Toolbox.
•
This completes Lab2.
TM
External Use
105
Optional Lab 3, Step 1: Let’s modify a few things
In Sources/drivers.c
Add:
int16 iChi; // tilt angle
at the top of function CreateAndSendBlueToothPacketsViaUART()
Append statements to look up iChi to each of the case options of switch(globals.QuaternionPacketType) . The
7 statements needed are:
iChi = (int16) (10.0F * thisSV_3DOF_G_BASIC.fLPChi); // Q3
iChi = (int16) (10.0F * thisSV_3DOF_B_BASIC.fLPChi); // Q3M
iChi = (int16) (10.0F * thisSV_3DOF_Y_BASIC.fChi); // Q3G
iChi = (int16) (10.0F * thisSV_6DOF_GB_BASIC.fLPChi); //Q6MA
iChi = (int16) (10.0F * thisSV_6DOF_GY_KALMAN.fChiPl); // Q6AG
iChi = (int16) (10.0F * thisSV_9DOF_GBY_KALMAN.fChiPl); // Q9
iChi = 0; // NOT IMPLEMENTED. THIS IS A PLACEHOLDER // QCC
In the “if (globals.RPCPacketOn) section, replace:
sBufAppendItem(sUARTOutputBuf, &iIndex, (uint8*)&iPhi, 2);
sBufAppendItem(sUARTOutputBuf, &iIndex, (uint8*)&iThe, 2);
sBufAppendItem(sUARTOutputBuf, &iIndex, (uint8*)&iRho, 2);
with
int16 zero, compassPoint;
// Use iChi instead of iPhi
// Convert compass heading to a cruder N, NE, E, SE, S, SW, W, NW heading
// [12-7]: add the angles (resolution 0.1 deg per count) to the transmit buffer
zero = 0;
compassPoint = iRho-22.5;
compassPoint = compassPoint/450;
compassPoint = compassPoint * 450;
sBufAppendItem(sUARTOutputBuf, &iIndex, (uint8*)&iChi, 2);
sBufAppendItem(sUARTOutputBuf, &iIndex, (uint8*)&zero, 2);
sBufAppendItem(sUARTOutputBuf, &iIndex, (uint8*)&compassPoint, 2);
TM
External Use
106
Lab 3, Step 2: Rebuild & experiment
What should be the effect of the changes on the prior page?
Hint: iChi is tilt angle in degrees
Rebuild the project
b) Download and experiment with changes via the “Dynamics” tab
in the Freescale Sensor Fusion Toolbox running on your PC
a)
Don’t forget to refer to the slides which specify available fusion
outputs.
This concludes the 3nd lab.
TM
External Use
107
Reminder: Global Data Structures
Pointer Function
Structure Name
Structure Type
defined in include file
Accelerometer
thisAccel
AccelSensor
proj_config.h
Magnetometer
thisMag
MagSensor
Gyroscope
thisGyro
GyroSensor
3-axis results
thisSV_3DOF_G_BASIC
SV_3DOF_G_BASIC
eCompass results
thisSV_6DOF_GB_BASIC
SB_6DOF_GB_BASIC
accel+gyro results
thisSV_6DOF_GY_KALMAN
SV_6DOF_GY_KALMAN
9-axis results
thisSV_9DOF_GBY_KALMAN SV_9DOF_GBY_KALMAN
TM
External Use
108
tasks_func.h
Reminder: Location of variables within the global
structures
Description
data type
roll in degrees
pitch in degrees
yaw in degrees
compass heading in degrees
tilt angle in degrees
magnetic inclination angle in
degrees
geomagnetic vector
(microTeslas, global frame)
gyro offset in degrees/sec
linear acceleration in the
sensor frame in gravities
linear acceleration in the global
frame in gravities
quaternion (unitless)
float
float
float
float
float
float
G
(accel)
fLPPhi
fLPThe
fLPPsi
fLPRho
fLPChi
N/A
float
N/A
Fusion Algorithm Options
GB
GY
(eCompass)
(accel + gyro)
fLPPhi
fPhiPl
fLPThe
fThePl
fLPPsi
fPsiPl
fLPRho
fRhoPl
fLPChi
fChiPl
fDelta
N/A
fLPDelta
N/A
N/A
float
float
N/A
N/A
N/A
N/A
fbPl[3]
faSePl[3]
fbPL[3]
faSePl[3]
float
N/A
N/A
N/A
fq
fLPq
fOmega[3]1
fR[3][3]
fLPR[3][3]
fLPRVec[3]
fdeltat
fq
fLPq
fOmega[3]
fR[3][3]
fLPR[3][3]
fLPRvec[3]
fdeltat
fqPl
faGlPl[3]
fLPaGlPl[3]
fqPl
fOmega[3]2
fRPl[3][3]
fOmega[3] 2
fRPl[3][3]
fRVecPl[3]
fdeltat
fRVecPl[3]
fdeltat
fquaternion
angular velocity in dps
orientation matrix (unitless)
float
float
rotation vector
time interval in seconds
float
float
TM
External Use
109
GBY
9-axis
fPhiPl
fThePl
fPsiPl
fRhoPl
fChiPl
fDeltaPl
fmGl[3]
Q & A Opportunity
TM
External Use
110
Part 8: Odds & Ends & Review
TM
External Use
111
In summary
Freescale offers the lowest cost, most complete, sensor fusion solution
available anywhere, with:
• BSD style open source license (it’s FREE!)
• 3, 6 and 9-axis sensor fusion options
• Source code for all functions
• Working template programs
• Low cost hardware options
• Extensive documentation (data sheet, user manual and multiple app
notes, training slides and videos)
• Free Windows and Android applications to visualize fusion results
• Freescale community support at
https://community.freescale.com/community/sensors/sensorfusion
• Paid support available from Freescale’s Software Services team
([email protected])
• For more details, please visit http://www.freescale.com/sensorfusion
TM
External Use
112
More Information on Freescale Sensor Solutions
•
•
http://www.freescale.com/freedom
http://www.freescale.com/gyro
http://www.freescale.com/sensors
http://www.freescale.com/sensortoolbox
www.twitter.com/Sensorfusion
•
Blogs: Smart Mobile Devices
•
•
•
− http://blogs.freescale.com/author/michaelestanley/
•
Android App available on Google Play
− Freescale
Sensor Fusion Toolbox
http://www.freescale.com/sensorfusion
TM
External Use
113
Additional Resources
•
•
•
•
•
•
•
•
Orientation Representations: Part 1
Orientation Representations: Part 2
Hard and soft iron magnetic compensation explained
Freescale E-Compass Software
“Euler Angles” at http://en.wikipedia.org/wiki/Euler_Angles
“Introduction to Random Signals and Applied Kalman Filtering”, 3rd
edition, by Robert Grover brown and Patrick Y.C. Hwang, John
Wiley & Sons, 1997.
“Quaternions and Rotation Sequences”, Jack B. Kuipers, Princeton
University Press, 1999.
Matlab computer software by MathWorks http://www.mathworks.com/products/matlab/
TM
External Use
114
Wrap-up
In this course, we have:
− Learned some motion sensor basics
− Learned what “orientation” is
− Reviewed a basic introduction to motion sensor fusion
− Learned about Freescale’s Freescale Sensor Fusion Library, and how
we might use it to create our own custom functions
− Experimented with the Freescale Sensor Fusion Toolbox
− Learned where to look for more information
TM
External Use
115
Thank you for your time and
interest.
TM
External Use
116
TM
www.Freescale.com
© 2014 Freescale Semiconductor, Inc. | External Use
Auxiliary Slides
TM
External Use
118
Use the right rotation representation at each stage of your
calculation
Topic
Quaternion
Rotation Matrix
Storage
Requires 16 bytes of storage in single
precision floating point (4 elements at 4 bytes
each)
Requires 36 bytes of storage (9 elements at
4 bytes each)
Computation (for
2 sequential
rotations)
4 elements each requiring 4 multiplies and 3
additions = 28 operations
9 elements, each requiring 3 multiplies and 2
additions = 45 operations
Vector rotation
Rotating a vector by pre- and postmultiplication of quaternion requires 52
operations
Rotating a vector via rotation matrix requires
15 operations (3 elements each requiring 3
multiplies and 2 additions)
Discontinuities
° about any axis of rotation XYZ is
equivalent to -° about axis of rotation -XYZ .
None
Ease of
Understanding
Generally takes a lot of study to understand
the details
From rotation matrix =
Conversion
m11
m12
m13
m21
m22
m23
m31
we have:
q0 = 0.5 sqrt(m11 + m22 + m33 + 1)
q1 = (m32 - m23) / (4q0)
q2 = (m13 - m31) / (4q0)
q3 = (m21 - m12) / (4q0)
m32
m33
TM
External Use
119
Easily understood by most engineers
RM =
2q02 - 1 + 2q12
2q1q2 - 2q0q3
2q1q3 +2q0q2
2q1q2 + 2q0q3
2q02 - 1 + 2q22
2q2q3 - 2q0q1
2q1q3 - 2q0q2
2q2q3 + 2q0q1
2q02 -1 + 2q32
A couple of really useful math identities
If a and b are 3x1 vectors, then
a
•
The dot product (ab) is a scalar:

− ab = a1b1 + a2b2 + a3b3 = |a||b| cos 
|a|cos
−  is the angle between the two vectors = cos-1(ab / (|a||b|))
•
The cross product (a X b) is another vector:
−
a X b = |a||b| sin  n, where n is a unit vector perpendicular to the
plane containing a and b
TM
External Use
120
b
tasks.c
•
Defines the following functions:
−
RdSensData_Init (void)
− RdSensData_Run (void)
− Fusion_Init (void)
− Fusion Run (void)
− MagCal_Run (void)
− ApplyHal (struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct
•
These are the main
functions called from MQX
GyroSensor *pthisGyro, int32 irow)
Compile options for tasks.c are responsible for binding in various
algorithms into the final application
TM
External Use
121
Project Configuration
•
build.h contains standard defines to control the build process
− THISCOORDINATESYSTEM
= NED | ANDROID | WIN8
− Boolean controls (uncomment #define to enable):
#define name
Function
DEEPSLEEP
Enable deep sleep in idle task()
UART_OFF
Disables UART communication for power
measurements
COMPUTE_3DOF_G_BASIC
Enable 3-axis accelerometer tilt algorithm
COMPUTE_6DOF_GB_BASIC
Enable 6-axis accel/mag eCompass algorithm
COMPUTE_6DOF_GY_KALMAN
Enable 6-axis accel/gyro Kalman algorithm
COMPUTE_9DOF_GBY_KALMAN
Enable 9-axis Kalman algorithm
TM
External Use
122
Project Configuration
#define SENSORFS 200 // int32: frequency (Hz) of sensor sampling process
#define OVERSAMPLE_RATIO 8 // ODR = SENSORFS/OVERSAMPLE_RATIO
Other configuration file changes are best made by the Freescale
software and services team
TM
External Use
123
Events.c
•
NMI interrupt handlers (not used)
• Low frequency counter restart
• UART control functions
− UART_On-BlockReceived()
is where the application command
interpreter is located
− This is example code only, not a formal part of the fusion library
TM
External Use
124
drivers.c major functions
FXOS8700_Init() initializes the FXOS87000CQ combo sensor
FXAS21000_Init() initializes the FXAS21000 gyro
MMA8652_Init() initializes the MMA8652 accelerometer
MAG3110_Init() initializes the MAG3310 magnetometer
FXAS21000_ReadData()
FXOS8700_ReadData()
MMA8652_ReadData()
MAG3110_ReadData()
CreateAndSendBluetoothPacketsViaUART() sends data packets via Bluetooth
TM
External Use
125
mqx_tasks.c
•
Main_task() sets up periodic tasks then exits
• RdSensData_task() is the high frequency sample task
• Fusion_task() is the medium frequency fusion task
− flash
green LED
− calls Fusion_Run()
− send new packet via Bluetooth via CreateAndSendBluetooth
PacketsViaUART()
− set MagCal event as necessary
•
MagCal_task()
− flash
red LED
− run MagCal_run(), which is part of the fusion library
TM
External Use
126
main.c
•
“C” main()
− PE_low_level_init()
− PEX_RTOS_START()
TM
External Use
127
Dependencies Between Project & Fusion Library/Source
Calling Function
Calling Function File
Calls
From
RdSensData_Init
tasks.c
MPL3115_Init
FXOS8700_Init
FXAS21000_Init
MMA8652_Init
MAG3110_Init
drivers.c
RdSensData_Run
MPL3115_ReadData
FXOS8700_ReadData
FXAS21000_ReadData
MMA8652_ReadData
MAG3110_ReadData
RdSensData_task
mqx_tasks.c
RdSensData_Run
RdSensData_Init
Fusion_task
Fusion_Init
Fusion_Run
MagCal_task
MagCal_Run
TM
External Use
128
tasks.c
TM
www.Freescale.com
© 2014 Freescale Semiconductor, Inc. | External Use
Was this manual useful for you? yes no
Thank you for your participation!

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement