SweetMaker MotionSensor Library Tutorial

1/*******************************************************************************
2   Tutorial.ino - This is a MotionSensor example sketch which shows how to use it
3                  and how to use Quaternions for robust and efficient motion
4                  processing
5
6  Copyright(C) 2022  Howard James May
7
8   This file is part of the SweetMaker SDK
9
10  The SweetMaker SDK is free software:   you can redistribute it and / or
11  modify it under the terms of the GNU General   Public License as published by
12  the Free Software Foundation, either version   3 of the License, or
13  (at your option) any later version.
14
15  The SweetMaker   SDK is distributed in the hope that it will be useful,
16  but WITHOUT ANY WARRANTY;   without even the implied warranty of
17  MERCHANTABILITY or FITNESS FOR A PARTICULAR   PURPOSE.See the
18  GNU General Public License for more details.
19
20  You should   have received a copy of the GNU General Public License
21  along with this program.If   not, see <http://www.gnu.org/licenses/>.
22
23  Contact me at sweet.maker@outlook.com
24
25********************************************************************************
26Release      Date                        Change Description
27--------|-------------|--------------------------------------------------------|
28    1      07-Mar-2022   Initial release
29*******************************************************************************/
30
31#include   <Wire.h>
32#include <SweetMaker.h>
33#include <MotionSensor.h>
34
35using namespace   SweetMaker;
36
37/*
38 * Motion Sensor instance for our use.
39 */
40MotionSensor   motionSensor;
41
42/*
43 * Function prototypes used in this sketch
44 */
45void   myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value);
46void   handleSerialInput();
47
48/*
49 * RunModes used in this tutorial. These correspond   to different outputs 
50 */
51enum RunMode { 
52  IDLE = 0, 
53  QUATERNION   = 1,
54  YAW_PITCH_ROLL = 2,
55  GRAVITY = 3,
56  TILT_X_Y = 4,
57  TILT_X_Y_NON_TRIG   = 5,
58  ROTATIONAL_SPEED = 6,
59  VECTOR_ANGLE = 7,
60} runMode = IDLE;
61
62/*
63   * Arduino framework setup function - called once at system start
64 */
65void   setup()
66{
67  Serial.begin(115200); // Serial connection
68  Serial.println("Motion   Sensor & Quaternion Tutorial");
69
70  /*
71   * Register eventHandler with   SweetMaker Framework
72   */
73  EventMngr::getMngr()->configCallBack(myEventHandlerCallback);
74
75   /*
76   * Initialise motionSensor - this establishes comms over I2C with
77    * the MPU6050 and downloads code to the MPU for motion processing
78   * allowing   generation of rotation quaternion. 
79   */
80  if(motionSensor.init() != 0) {
81     // try again
82  }
83
84  /*
85   * While this sketch doesn't use TimerTick   events we give it a kick
86   * to start the generation of events.
87   */
88   TimerTickMngt::getTimerMngt()->update(0);
89
90  Serial.println("Setup complete");
91}
92
93/*
94   * Arduino framework main loop
95 *
96 * This function does little more than:
97   *     1) Calculate loop elapsed time,
98 *     2) Watch for input on the serial   port 
99 *     3) Call SweetMaker update function
100 *
101 * The Motion Sensor   is part of the SweetMaker framework and inherits from
102 * AutoUpdate - this integrates   it with the Auto Update process which 
103 * triggers the checking for new sensor   readings.
104 *
105 */
106void loop()
107{
108  static unsigned long lastUpdateTime_ms   = millis();
109  unsigned long thisTime_ms = millis();
110  unsigned long elapsedTime_ms   = thisTime_ms - lastUpdateTime_ms;
111
112  handleSerialInput();
113
114  AutoUpdateMngr::getUpdater()->update(elapsedTime_ms);
115
116   lastUpdateTime_ms = thisTime_ms;
117}
118
119/*
120 * myEventHandlerCallback:   This captures events from the SweetMaker Framework
121 *                         we   are only interested in:
122 *                           - MOTION_SENSOR_NEW_SMPL_RDY
123   *                           - MOTION_SENSOR_INIT_ERROR
124 *
125 * This function   switches on runmode state and demonstrates different motion processing
126 * accordingly.   The output is sent to the Serial port for the Arduino Serial Plotter.
127 */
128void   myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value)
129{
130   switch (event)
131  {
132  case MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY:
133     switch (runMode) {
134
135      /*
136       * The MPU6050 returns a Rotational   Quaternion which represents how the sensor has been rotated.
137       * See the   following link for a full explanation.
138       * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
139        * As the sensor moves the Rotational Quaternion updates accordingly.
140        *
141       * Note: the rotational quaternion doesn't immediately say how   a particular vector
142       * has changed - this must be calculated using the   Rotational Quaternion
143       *
144       * Also: the MPU6050 doesn't have a magnetometer   and thus no concept of true North,
145       * consequently it's hortizontal orientation   is arbitrary and will drift
146       *
147       * The Rotational Quaternion indicates   the amount of rotation (r) about an axis (x,y,z)
148       *
149       * The MPU6050   provides this as 14 bit signed integer values +/-16384 
150       *
151       *   These values have little use on their own without further processing.
152       */
153     case QUATERNION:
154    {
155      Serial.print("r:"); Serial.print(motionSensor.rotQuat.r);   Serial.print("\	");
156      Serial.print("x:"); Serial.print(motionSensor.rotQuat.x);   Serial.print("\	");
157      Serial.print("y:"); Serial.print(motionSensor.rotQuat.y);   Serial.print("\	");
158      Serial.print("z:"); Serial.print(motionSensor.rotQuat.z);   Serial.println();
159    }
160    break;
161
162    /*
163     * One common representation   of 3D orientation is Yaw Pitch Roll. 
164     *     - Yaw is horizontal orientation   - rotation about Z axis
165     *     - Pitch is forward backward tilt - rotation   about Y axis    
166     *     - Roll is side to side tilt - rotation about X axis
167      *
168     * These values can be calculated by MotionSensor from the Rotational   Quaternion.
169     * Note that defining pitch as rotation about Y is arbitrary   etc.
170     * Also note that this uses trigonometric functions to calculate and   so takes a 
171     * little time to compute. This may be fine for many applications.
172      *
173     * Reminder that Yaw will drift as the MPU6050 has no magnetometer
174      */
175    case YAW_PITCH_ROLL:
176    {
177      unsigned long start_us = micros();
178       int16_t yaw = motionSensor.rotQuat.getRotZ();
179      int16_t pitch = motionSensor.rotQuat.getRotY();
180       int16_t roll = motionSensor.rotQuat.getRotX();
181      unsigned long stop_us   = micros();
182
183      Serial.print(stop_us - start_us); Serial.print("\	");
184       Serial.print(yaw); Serial.print("\	");
185      Serial.print(pitch); Serial.print("\	");
186       Serial.print(roll); Serial.println();
187    }
188    break;
189
190    /*
191      * The MPU6050 is great for calculating Gravity and the lack of
192     * magnetometer   is no limitation for this. MotionSensor supports
193     * returning Gravity and   this has small computational expense 
194     * using no trigonometric functions.
195      *
196     */
197    case GRAVITY:
198    {
199      Quaternion_16384 gravity;
200       unsigned long start_us = micros();
201      motionSensor.rotQuat.getGravity(&gravity);
202       unsigned long stop_us = micros();
203
204      Serial.print(stop_us - start_us);   Serial.print("\	");
205      Serial.print(gravity.x); Serial.print("\	");
206       Serial.print(gravity.y); Serial.print("\	");
207      Serial.print(gravity.z);   Serial.println();
208    }
209    break;
210
211    /*
212     * As previously shown   Tilt X and Tilt Y (Pitch and Roll)
213     * can be produced.
214     */
215    case   TILT_X_Y:
216    {
217      unsigned long start_us = micros();
218      int16_t   tilt_forward = motionSensor.rotQuat.getRotX();
219      int16_t tilt_side = motionSensor.rotQuat.getRotY();
220       unsigned long stop_us = micros();
221
222      Serial.print(stop_us - start_us);   Serial.print("\	");
223      Serial.print(tilt_forward); Serial.print("\	");
224       Serial.print(tilt_side); Serial.println("\	0");
225    }
226    break;
227
228     /*
229     * In order to avoid the use of trigonometric functions it may be
230      * adequate to use the Sin of RotX and Sin RotY. This can be calculated
231      * using only multiplcations and right shifts.
232     */
233    case TILT_X_Y_NON_TRIG:
234     {
235      unsigned long start_us = micros();
236      int16_t tilt_forward   = motionSensor.rotQuat.getSinRotX();
237      int16_t tilt_side = motionSensor.rotQuat.getSinRotY();
238       unsigned long stop_us = micros();
239
240      Serial.print(stop_us - start_us);   Serial.print("\	");
241      Serial.print(tilt_forward); Serial.print("\	");
242       Serial.print(tilt_side); Serial.println("\	0");
243    }
244    break;
245
246     /*
247     * The rotational quaternion produced by the MPU6050 can also
248      * provide an indication of rotational speed by looking at the 
249     * rotational   quaternion delta and the angular part of that.
250     *
251     * The angular part   is equal to cos(theta/2), theta being the 
252     * angular rotation between the   two last readings. As the sample period
253     * is a fraction of a second, theta   is likely to be small and cos(theta/2)
254     * smaller still. The value should   be handled accordingly.
255     *
256     * Note: the rotation speed here is not   about any one particular axis rather
257     * it is about any arbitraty axis.
258      */
259    case ROTATIONAL_SPEED:
260    {
261      int16_t rot_speed = 16384   - motionSensor.rotQuatDelta.r;
262      Serial.print(rot_speed); Serial.println("\	0\	0\	0");
263     }
264    break;
265
266    /*
267     * One of the most powerful uses of the   quaternion is finding 
268     * how close the sensor is to pointing in a particular   direction.
269     * The dot product between two positional quaternions tells us   the 
270     * cosine of the angle between the two vectors in 3D space. This 
271      * is calculated without the use of trigonometric functions.
272     *
273     *   As we have seen the MPU6050 provides a robust calculation of where the 
274     *   Z axis (gravity) is now pointing. Thus we choose to compare gravity 
275     * against   vectors we are interested in.
276     *
277     */
278    case VECTOR_ANGLE:
279     {
280      Quaternion_16384 gq;
281      motionSensor.rotQuat.getGravity(&gq);
282
283       Quaternion_16384 z_axis = { 0, 0, 0, 0x4000 };
284      Quaternion_16384 forward_right   = { 0,9459,9459,9459 };
285      Quaternion_16384 forward_left = { 0,9459,-9459,9459   };
286
287      int16_t up = z_axis.dotProduct(&gq);
288      int16_t fr = forward_right.dotProduct(&gq);
289       int16_t fl = forward_left.dotProduct(&gq);
290
291      Serial.print(up);   Serial.print("\	");
292      Serial.print(fr); Serial.print("\	");
293      Serial.print(fl);   Serial.println("\	0");
294    }
295    break;
296
297    default:
298      break;
299     }
300    break;
301
302  case MotionSensor::MOTION_SENSOR_INIT_ERROR:
303    Serial.print("Motion   Sensor Init Error: "); Serial.println(value);
304    break;
305
306  default:
307     break;
308  }
309
310}
311
312
313void handleSerialInput()
314{
315  if (!Serial.available())
316     return;
317
318  char inChar = Serial.read();
319
320  Serial.println("_:0\	_:0\	_:0\	_:0");
321
322   switch (inChar)
323  {
324
325  case '0':
326    runMode = IDLE;
327    break;
328
329   case '1':
330    Serial.println("r x y z");
331    runMode = QUATERNION;
332     break;
333
334  case '2':
335    Serial.println("time_us yaw pitch roll");
336     runMode = YAW_PITCH_ROLL;
337    break;
338
339  case '3':
340    Serial.println("time_us   x y z");
341    runMode = GRAVITY;
342    break;
343
344  case '4':
345    Serial.println("time_us   tilt_forward tilt_side");
346    runMode = TILT_X_Y;
347    break;
348
349  case   '5':
350    Serial.println("time_us tilt_forward tilt_side");
351    runMode =   TILT_X_Y_NON_TRIG;
352    break;
353
354  case '6':
355    Serial.println("rotational_speed");
356     runMode = ROTATIONAL_SPEED;
357    break;
358
359  case '7':
360    Serial.println("z_axis   forward_right forward_left");
361    runMode = VECTOR_ANGLE;
362    break;
363
364   case 'c':
365  {
366    /*
367     * Each MPU6050 requires it's own set of calibration   values
368     * These can be calculated by MotionSensor and these values
369     *   submited in the motionSensor.init() function.
370     *
371     * You may choose   to store calibration values in EEPROM so 
372     * so that they can be retrieved   at system start without 
373     * hardcoding them into the sketch (which only works   if you
374     * are using a single MPU6050). See the SweetMaker StrawberryString
375      * which does just this.
376     */
377    Serial.println("Calibrating Motion   Sensor");
378    MotionSensor::CALIBRATION calibration;
379    motionSensor.runSelfCalibrate(&calibration);
380   }
381  break;
382
383
384  /*
385   * The MPU6050 may not be mounted 100% flat   or may be mounted in 
386   * some arbitrary orientation and an offset applied to   make it
387   * work as if it were horizontal. This can be done using the 
388   *   MotionSensor auto level feature. This takes advantage of the 
389   * RotationalQuaternions   ability to easily combine rotations.
390   *
391   * This works by finding the rotation   beteen gravity and the Z axis
392   * and using this as an offset.
393   *
394   *   https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
395    */
396  case 'l':
397    Serial.println("Self Leveling");
398    motionSensor.autoLevel();
399     break;
400
401    /*
402     * AutoLeveling can compensate for non-horizontal   mounting
403     * but a rotation about Z requires specific configuration.
404     *   Here we autolevel and also apply a 45 degree rotation offset.
405     */
406  case   'z':
407  {
408    Serial.println("Self Leveling + Z Rotation of 45 degrees");
409
410     float rotation_z = 45;
411
412    Quaternion_16384 gravity;
413    RotationQuaternion_16384   offsetQ;
414
415    RotationQuaternion_16384 rotZ(rotation_z, 0, 0, 16384);
416     Quaternion_16384 zAxis(0, 0, 0, 16384);
417
418    motionSensor.clearOffsetRotation();
419     motionSensor.rotQuat.getGravity(&gravity);
420
421    offsetQ.findOffsetRotation(&zAxis,   &gravity);
422    offsetQ.crossProduct(&rotZ);
423
424    motionSensor.setOffsetRotation(&offsetQ);
425   }
426  break;
427
428  default:
429    break;
430  }
431}
432
433
434

1/*******************************************************************************
2  Tutorial.ino - This is a MotionSensor example sketch which shows how to use it
3                 and how to use Quaternions for robust and efficient motion
4                 processing
5
6  Copyright(C) 2022  Howard James May
7
8  This file is part of the SweetMaker SDK
9
10  The SweetMaker SDK is free software:  you can redistribute it and / or
11  modify it under the terms of the GNU General  Public License as published by
12  the Free Software Foundation, either version  3 of the License, or
13  (at your option) any later version.
14
15  The SweetMaker  SDK is distributed in the hope that it will be useful,
16  but WITHOUT ANY WARRANTY;  without even the implied warranty of
17  MERCHANTABILITY or FITNESS FOR A PARTICULAR  PURPOSE.See the
18  GNU General Public License for more details.
19
20  You should  have received a copy of the GNU General Public License
21  along with this program.If  not, see <http://www.gnu.org/licenses/>.
22
23  Contact me at sweet.maker@outlook.com
24
25********************************************************************************
26Release     Date                        Change Description
27--------|-------------|--------------------------------------------------------|
28   1      07-Mar-2022   Initial release
29*******************************************************************************/
30
31#include  <Wire.h>
32#include <SweetMaker.h>
33#include <MotionSensor.h>
34
35using namespace  SweetMaker;
36
37/*
38 * Motion Sensor instance for our use.
39 */
40MotionSensor  motionSensor;
41
42/*
43 * Function prototypes used in this sketch
44 */
45void  myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value);
46void  handleSerialInput();
47
48/*
49 * RunModes used in this tutorial. These correspond  to different outputs 
50 */
51enum RunMode { 
52  IDLE = 0, 
53  QUATERNION  = 1,
54  YAW_PITCH_ROLL = 2,
55  GRAVITY = 3,
56  TILT_X_Y = 4,
57  TILT_X_Y_NON_TRIG  = 5,
58  ROTATIONAL_SPEED = 6,
59  VECTOR_ANGLE = 7,
60} runMode = IDLE;
61
62/*
63  * Arduino framework setup function - called once at system start
64 */
65void  setup()
66{
67  Serial.begin(115200); // Serial connection
68  Serial.println("Motion  Sensor & Quaternion Tutorial");
69
70  /*
71   * Register eventHandler with  SweetMaker Framework
72   */
73  EventMngr::getMngr()->configCallBack(myEventHandlerCallback);
74
75  /*
76   * Initialise motionSensor - this establishes comms over I2C with
77   * the MPU6050 and downloads code to the MPU for motion processing
78   * allowing  generation of rotation quaternion. 
79   */
80  if(motionSensor.init() != 0) {
81    // try again
82  }
83
84  /*
85   * While this sketch doesn't use TimerTick  events we give it a kick
86   * to start the generation of events.
87   */
88  TimerTickMngt::getTimerMngt()->update(0);
89
90  Serial.println("Setup complete");
91}
92
93/*
94  * Arduino framework main loop
95 *
96 * This function does little more than:
97  *     1) Calculate loop elapsed time,
98 *     2) Watch for input on the serial  port 
99 *     3) Call SweetMaker update function
100 *
101 * The Motion Sensor  is part of the SweetMaker framework and inherits from
102 * AutoUpdate - this integrates  it with the Auto Update process which 
103 * triggers the checking for new sensor  readings.
104 *
105 */
106void loop()
107{
108  static unsigned long lastUpdateTime_ms  = millis();
109  unsigned long thisTime_ms = millis();
110  unsigned long elapsedTime_ms  = thisTime_ms - lastUpdateTime_ms;
111
112  handleSerialInput();
113
114  AutoUpdateMngr::getUpdater()->update(elapsedTime_ms);
115
116  lastUpdateTime_ms = thisTime_ms;
117}
118
119/*
120 * myEventHandlerCallback:  This captures events from the SweetMaker Framework
121 *                         we  are only interested in:
122 *                           - MOTION_SENSOR_NEW_SMPL_RDY
123  *                           - MOTION_SENSOR_INIT_ERROR
124 *
125 * This function  switches on runmode state and demonstrates different motion processing
126 * accordingly.  The output is sent to the Serial port for the Arduino Serial Plotter.
127 */
128void  myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value)
129{
130  switch (event)
131  {
132  case MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY:
133    switch (runMode) {
134
135      /*
136       * The MPU6050 returns a Rotational  Quaternion which represents how the sensor has been rotated.
137       * See the  following link for a full explanation.
138       * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
139       * As the sensor moves the Rotational Quaternion updates accordingly.
140       *
141       * Note: the rotational quaternion doesn't immediately say how  a particular vector
142       * has changed - this must be calculated using the  Rotational Quaternion
143       *
144       * Also: the MPU6050 doesn't have a magnetometer  and thus no concept of true North,
145       * consequently it's hortizontal orientation  is arbitrary and will drift
146       *
147       * The Rotational Quaternion indicates  the amount of rotation (r) about an axis (x,y,z)
148       *
149       * The MPU6050  provides this as 14 bit signed integer values +/-16384 
150       *
151       *  These values have little use on their own without further processing.
152       */
153    case QUATERNION:
154    {
155      Serial.print("r:"); Serial.print(motionSensor.rotQuat.r);  Serial.print("\	");
156      Serial.print("x:"); Serial.print(motionSensor.rotQuat.x);  Serial.print("\	");
157      Serial.print("y:"); Serial.print(motionSensor.rotQuat.y);  Serial.print("\	");
158      Serial.print("z:"); Serial.print(motionSensor.rotQuat.z);  Serial.println();
159    }
160    break;
161
162    /*
163     * One common representation  of 3D orientation is Yaw Pitch Roll. 
164     *     - Yaw is horizontal orientation  - rotation about Z axis
165     *     - Pitch is forward backward tilt - rotation  about Y axis    
166     *     - Roll is side to side tilt - rotation about X axis
167     *
168     * These values can be calculated by MotionSensor from the Rotational  Quaternion.
169     * Note that defining pitch as rotation about Y is arbitrary  etc.
170     * Also note that this uses trigonometric functions to calculate and  so takes a 
171     * little time to compute. This may be fine for many applications.
172     *
173     * Reminder that Yaw will drift as the MPU6050 has no magnetometer
174     */
175    case YAW_PITCH_ROLL:
176    {
177      unsigned long start_us = micros();
178      int16_t yaw = motionSensor.rotQuat.getRotZ();
179      int16_t pitch = motionSensor.rotQuat.getRotY();
180      int16_t roll = motionSensor.rotQuat.getRotX();
181      unsigned long stop_us  = micros();
182
183      Serial.print(stop_us - start_us); Serial.print("\	");
184      Serial.print(yaw); Serial.print("\	");
185      Serial.print(pitch); Serial.print("\	");
186      Serial.print(roll); Serial.println();
187    }
188    break;
189
190    /*
191     * The MPU6050 is great for calculating Gravity and the lack of
192     * magnetometer  is no limitation for this. MotionSensor supports
193     * returning Gravity and  this has small computational expense 
194     * using no trigonometric functions.
195     *
196     */
197    case GRAVITY:
198    {
199      Quaternion_16384 gravity;
200      unsigned long start_us = micros();
201      motionSensor.rotQuat.getGravity(&gravity);
202      unsigned long stop_us = micros();
203
204      Serial.print(stop_us - start_us);  Serial.print("\	");
205      Serial.print(gravity.x); Serial.print("\	");
206      Serial.print(gravity.y); Serial.print("\	");
207      Serial.print(gravity.z);  Serial.println();
208    }
209    break;
210
211    /*
212     * As previously shown  Tilt X and Tilt Y (Pitch and Roll)
213     * can be produced.
214     */
215    case  TILT_X_Y:
216    {
217      unsigned long start_us = micros();
218      int16_t  tilt_forward = motionSensor.rotQuat.getRotX();
219      int16_t tilt_side = motionSensor.rotQuat.getRotY();
220      unsigned long stop_us = micros();
221
222      Serial.print(stop_us - start_us);  Serial.print("\	");
223      Serial.print(tilt_forward); Serial.print("\	");
224      Serial.print(tilt_side); Serial.println("\	0");
225    }
226    break;
227
228    /*
229     * In order to avoid the use of trigonometric functions it may be
230     * adequate to use the Sin of RotX and Sin RotY. This can be calculated
231     * using only multiplcations and right shifts.
232     */
233    case TILT_X_Y_NON_TRIG:
234    {
235      unsigned long start_us = micros();
236      int16_t tilt_forward  = motionSensor.rotQuat.getSinRotX();
237      int16_t tilt_side = motionSensor.rotQuat.getSinRotY();
238      unsigned long stop_us = micros();
239
240      Serial.print(stop_us - start_us);  Serial.print("\	");
241      Serial.print(tilt_forward); Serial.print("\	");
242      Serial.print(tilt_side); Serial.println("\	0");
243    }
244    break;
245
246    /*
247     * The rotational quaternion produced by the MPU6050 can also
248     * provide an indication of rotational speed by looking at the 
249     * rotational  quaternion delta and the angular part of that.
250     *
251     * The angular part  is equal to cos(theta/2), theta being the 
252     * angular rotation between the  two last readings. As the sample period
253     * is a fraction of a second, theta  is likely to be small and cos(theta/2)
254     * smaller still. The value should  be handled accordingly.
255     *
256     * Note: the rotation speed here is not  about any one particular axis rather
257     * it is about any arbitraty axis.
258     */
259    case ROTATIONAL_SPEED:
260    {
261      int16_t rot_speed = 16384  - motionSensor.rotQuatDelta.r;
262      Serial.print(rot_speed); Serial.println("\	0\	0\	0");
263    }
264    break;
265
266    /*
267     * One of the most powerful uses of the  quaternion is finding 
268     * how close the sensor is to pointing in a particular  direction.
269     * The dot product between two positional quaternions tells us  the 
270     * cosine of the angle between the two vectors in 3D space. This 
271     * is calculated without the use of trigonometric functions.
272     *
273     *  As we have seen the MPU6050 provides a robust calculation of where the 
274     *  Z axis (gravity) is now pointing. Thus we choose to compare gravity 
275     * against  vectors we are interested in.
276     *
277     */
278    case VECTOR_ANGLE:
279    {
280      Quaternion_16384 gq;
281      motionSensor.rotQuat.getGravity(&gq);
282
283      Quaternion_16384 z_axis = { 0, 0, 0, 0x4000 };
284      Quaternion_16384 forward_right  = { 0,9459,9459,9459 };
285      Quaternion_16384 forward_left = { 0,9459,-9459,9459  };
286
287      int16_t up = z_axis.dotProduct(&gq);
288      int16_t fr = forward_right.dotProduct(&gq);
289      int16_t fl = forward_left.dotProduct(&gq);
290
291      Serial.print(up);  Serial.print("\	");
292      Serial.print(fr); Serial.print("\	");
293      Serial.print(fl);  Serial.println("\	0");
294    }
295    break;
296
297    default:
298      break;
299    }
300    break;
301
302  case MotionSensor::MOTION_SENSOR_INIT_ERROR:
303    Serial.print("Motion  Sensor Init Error: "); Serial.println(value);
304    break;
305
306  default:
307    break;
308  }
309
310}
311
312
313void handleSerialInput()
314{
315  if (!Serial.available())
316    return;
317
318  char inChar = Serial.read();
319
320  Serial.println("_:0\	_:0\	_:0\	_:0");
321
322  switch (inChar)
323  {
324
325  case '0':
326    runMode = IDLE;
327    break;
328
329  case '1':
330    Serial.println("r x y z");
331    runMode = QUATERNION;
332    break;
333
334  case '2':
335    Serial.println("time_us yaw pitch roll");
336    runMode = YAW_PITCH_ROLL;
337    break;
338
339  case '3':
340    Serial.println("time_us  x y z");
341    runMode = GRAVITY;
342    break;
343
344  case '4':
345    Serial.println("time_us  tilt_forward tilt_side");
346    runMode = TILT_X_Y;
347    break;
348
349  case  '5':
350    Serial.println("time_us tilt_forward tilt_side");
351    runMode =  TILT_X_Y_NON_TRIG;
352    break;
353
354  case '6':
355    Serial.println("rotational_speed");
356    runMode = ROTATIONAL_SPEED;
357    break;
358
359  case '7':
360    Serial.println("z_axis  forward_right forward_left");
361    runMode = VECTOR_ANGLE;
362    break;
363
364  case 'c':
365  {
366    /*
367     * Each MPU6050 requires it's own set of calibration  values
368     * These can be calculated by MotionSensor and these values
369     *  submited in the motionSensor.init() function.
370     *
371     * You may choose  to store calibration values in EEPROM so 
372     * so that they can be retrieved  at system start without 
373     * hardcoding them into the sketch (which only works  if you
374     * are using a single MPU6050). See the SweetMaker StrawberryString
375     * which does just this.
376     */
377    Serial.println("Calibrating Motion  Sensor");
378    MotionSensor::CALIBRATION calibration;
379    motionSensor.runSelfCalibrate(&calibration);
380  }
381  break;
382
383
384  /*
385   * The MPU6050 may not be mounted 100% flat  or may be mounted in 
386   * some arbitrary orientation and an offset applied to  make it
387   * work as if it were horizontal. This can be done using the 
388   *  MotionSensor auto level feature. This takes advantage of the 
389   * RotationalQuaternions  ability to easily combine rotations.
390   *
391   * This works by finding the rotation  beteen gravity and the Z axis
392   * and using this as an offset.
393   *
394   *  https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
395   */
396  case 'l':
397    Serial.println("Self Leveling");
398    motionSensor.autoLevel();
399    break;
400
401    /*
402     * AutoLeveling can compensate for non-horizontal  mounting
403     * but a rotation about Z requires specific configuration.
404     *  Here we autolevel and also apply a 45 degree rotation offset.
405     */
406  case  'z':
407  {
408    Serial.println("Self Leveling + Z Rotation of 45 degrees");
409
410    float rotation_z = 45;
411
412    Quaternion_16384 gravity;
413    RotationQuaternion_16384  offsetQ;
414
415    RotationQuaternion_16384 rotZ(rotation_z, 0, 0, 16384);
416    Quaternion_16384 zAxis(0, 0, 0, 16384);
417
418    motionSensor.clearOffsetRotation();
419    motionSensor.rotQuat.getGravity(&gravity);
420
421    offsetQ.findOffsetRotation(&zAxis,  &gravity);
422    offsetQ.crossProduct(&rotZ);
423
424    motionSensor.setOffsetRotation(&offsetQ);
425  }
426  break;
427
428  default:
429    break;
430  }
431}
432
433
434