1 | #include "r2ca.h"
|
---|
2 |
|
---|
3 | /****************************************************************************
|
---|
4 | * Copyright (C) 2011 - 2014 Bosch Sensortec GmbH
|
---|
5 | *
|
---|
6 | * Accelerometer.ino
|
---|
7 | * Date: 2014/09/09
|
---|
8 | * Revision: 3.0 $
|
---|
9 | *
|
---|
10 | * Usage: Example code to stream Accelerometer data
|
---|
11 | *
|
---|
12 | ****************************************************************************
|
---|
13 | /***************************************************************************
|
---|
14 | * License:
|
---|
15 | *
|
---|
16 | * Redistribution and use in source and binary forms, with or without
|
---|
17 | * modification, are permitted provided that the following conditions are met:
|
---|
18 | *
|
---|
19 | * Redistributions of source code must retain the above copyright
|
---|
20 | * notice, this list of conditions and the following disclaimer.
|
---|
21 | *
|
---|
22 | * Redistributions in binary form must reproduce the above copyright
|
---|
23 | * notice, this list of conditions and the following disclaimer in the
|
---|
24 | * documentation and/or other materials provided with the distribution.
|
---|
25 | *
|
---|
26 | * Neither the name of the copyright holder nor the names of the
|
---|
27 | * contributors may be used to endorse or promote products derived from
|
---|
28 | * this software without specific prior written permission.
|
---|
29 | *
|
---|
30 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
---|
31 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
---|
32 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
---|
33 | * DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
---|
34 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
---|
35 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
---|
36 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
---|
37 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
---|
38 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
---|
39 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
|
---|
40 | *
|
---|
41 | * The information provided is believed to be accurate and reliable.
|
---|
42 | * The copyright holder assumes no responsibility for the consequences of use
|
---|
43 | * of such information nor for any infringement of patents or
|
---|
44 | * other rights of third parties which may result from its use.
|
---|
45 | * No license is granted by implication or otherwise under any patent or
|
---|
46 | * patent rights of the copyright holder.
|
---|
47 | */
|
---|
48 |
|
---|
49 | #include "NAxisMotion.h" //Contains the bridge code between the API and the Arduino Environment
|
---|
50 | #include <Wire.h>
|
---|
51 |
|
---|
52 | NAxisMotion mySensor; //Object that for the sensor
|
---|
53 | unsigned long lastStreamTime = 0; //To store the last streamed time stamp
|
---|
54 | const int streamPeriod = 20; //To stream at 25Hz without using additional timers (time period(ms) =1000/frequency(Hz))
|
---|
55 |
|
---|
56 | //bool updateSensorData = true; //Flag to update the sensor data. Default is true to perform the first read before the first stream
|
---|
57 |
|
---|
58 | void setup() //This code is executed once
|
---|
59 | {
|
---|
60 | //Peripheral Initialization
|
---|
61 | Serial.begin(115200); //Initialize the Serial Port to view information on the Serial Monitor
|
---|
62 | I2C.begin(); //Initialize I2C communication to the let the library communicate with the sensor.
|
---|
63 | //Sensor Initialization
|
---|
64 | mySensor.initSensor(0x29); //The I2C Address can be changed here inside this function in the library
|
---|
65 | mySensor.setOperationMode(OPERATION_MODE_NDOF); //Can be configured to other operation modes as desired
|
---|
66 | mySensor.setUpdateMode(MANUAL); //The default is AUTO. Changing to manual requires calling the relevant update functions prior to calling the read functions
|
---|
67 |
|
---|
68 | Serial5.begin(115200);
|
---|
69 | Serial.print("Start!");
|
---|
70 | }
|
---|
71 |
|
---|
72 | float g_eula_h;
|
---|
73 | float g_eula_r;
|
---|
74 | float g_eula_p;
|
---|
75 |
|
---|
76 | void loop() //This code is looped forever
|
---|
77 | {
|
---|
78 | if ((millis() - lastStreamTime) >= streamPeriod)
|
---|
79 | {
|
---|
80 | lastStreamTime = millis();
|
---|
81 | mySensor.updateEuler(); //Update the Euler data into the structure of the object
|
---|
82 | mySensor.updateCalibStatus(); //Update the Calibration Status
|
---|
83 |
|
---|
84 | g_eula_h = mySensor.readEulerHeading();
|
---|
85 | g_eula_r = mySensor.readEulerRoll();
|
---|
86 | g_eula_p = mySensor.readEulerPitch();
|
---|
87 | #ifdef PRINT_SENSOR
|
---|
88 | Serial.print("Time: ");
|
---|
89 | Serial.print(lastStreamTime);
|
---|
90 | Serial.print("ms ");
|
---|
91 |
|
---|
92 | Serial.print(" H: ");
|
---|
93 | Serial.print(g_eula_h); //Heading(h) 0 to 360 data
|
---|
94 | Serial.print("deg ");
|
---|
95 |
|
---|
96 | Serial.print(" R: ");
|
---|
97 | Serial.print(g_eula_r); //Roll(y) 180 to -180 data
|
---|
98 | Serial.print("deg");
|
---|
99 |
|
---|
100 | Serial.print(" P: ");
|
---|
101 | Serial.print(g_eula_p); //Pitch(x) 180 to -180 data
|
---|
102 | Serial.print("deg ");
|
---|
103 |
|
---|
104 | Serial.print(" A: ");
|
---|
105 | Serial.print(mySensor.readAccelCalibStatus()); //Accelerometer Calibration Status (0 - 3)
|
---|
106 |
|
---|
107 | Serial.print(" M: ");
|
---|
108 | Serial.print(mySensor.readMagCalibStatus()); //Magnetometer Calibration Status (0 - 3)
|
---|
109 |
|
---|
110 | Serial.print(" G: ");
|
---|
111 | Serial.print(mySensor.readGyroCalibStatus()); //Gyroscope Calibration Status (0 - 3)
|
---|
112 |
|
---|
113 | Serial.print(" S: ");
|
---|
114 | Serial.print(mySensor.readSystemCalibStatus()); //System Calibration Status (0 - 3)
|
---|
115 |
|
---|
116 | Serial.println();
|
---|
117 | #endif
|
---|
118 | }
|
---|
119 |
|
---|
120 | #if 0
|
---|
121 | if (updateSensorData) //Keep the updating of data as a separate task
|
---|
122 | {
|
---|
123 | mySensor.updateAccel(); //Update the Accelerometer data
|
---|
124 | mySensor.updateLinearAccel(); //Update the Linear Acceleration data
|
---|
125 | mySensor.updateGravAccel(); //Update the Gravity Acceleration data
|
---|
126 | mySensor.updateCalibStatus(); //Update the Calibration Status
|
---|
127 | updateSensorData = false;
|
---|
128 | }
|
---|
129 |
|
---|
130 | if ((millis() - lastStreamTime) >= streamPeriod)
|
---|
131 | {
|
---|
132 | lastStreamTime = millis();
|
---|
133 | Serial.print("Time: ");
|
---|
134 | Serial.print(lastStreamTime);
|
---|
135 | Serial.print("ms ");
|
---|
136 |
|
---|
137 | Serial.print(" aX: ");
|
---|
138 | Serial.print(mySensor.readAccelX()); //Accelerometer X-Axis data
|
---|
139 | Serial.print("m/s2 ");
|
---|
140 |
|
---|
141 | Serial.print(" aY: ");
|
---|
142 | Serial.print(mySensor.readAccelY()); //Accelerometer Y-Axis data
|
---|
143 | Serial.print("m/s2 ");
|
---|
144 |
|
---|
145 | Serial.print(" aZ: ");
|
---|
146 | Serial.print(mySensor.readAccelZ()); //Accelerometer Z-Axis data
|
---|
147 | Serial.print("m/s2 ");
|
---|
148 |
|
---|
149 | Serial.print(" lX: ");
|
---|
150 | Serial.print(mySensor.readLinearAccelX()); //Linear Acceleration X-Axis data
|
---|
151 | Serial.print("m/s2 ");
|
---|
152 |
|
---|
153 | Serial.print(" lY: ");
|
---|
154 | Serial.print(mySensor.readLinearAccelY()); //Linear Acceleration Y-Axis data
|
---|
155 | Serial.print("m/s2 ");
|
---|
156 |
|
---|
157 | Serial.print(" lZ: ");
|
---|
158 | Serial.print(mySensor.readLinearAccelZ()); //Linear Acceleration Z-Axis data
|
---|
159 | Serial.print("m/s2 ");
|
---|
160 |
|
---|
161 | Serial.print(" gX: ");
|
---|
162 | Serial.print(mySensor.readGravAccelX()); //Gravity Acceleration X-Axis data
|
---|
163 | Serial.print("m/s2 ");
|
---|
164 |
|
---|
165 | Serial.print(" gY: ");
|
---|
166 | Serial.print(mySensor.readGravAccelY()); //Gravity Acceleration Y-Axis data
|
---|
167 | Serial.print("m/s2 ");
|
---|
168 |
|
---|
169 | Serial.print(" gZ: ");
|
---|
170 | Serial.print(mySensor.readGravAccelZ()); //Gravity Acceleration Z-Axis data
|
---|
171 | Serial.print("m/s2 ");
|
---|
172 |
|
---|
173 | Serial.print(" C: ");
|
---|
174 | Serial.print(mySensor.readAccelCalibStatus()); //Accelerometer Calibration Status (0 - 3)
|
---|
175 |
|
---|
176 | Serial.println();
|
---|
177 | updateSensorData = true;
|
---|
178 | }
|
---|
179 | #endif
|
---|
180 |
|
---|
181 | delay(1);
|
---|
182 | }
|
---|
183 |
|
---|
184 | bool processing_connected = false;
|
---|
185 |
|
---|
186 | void establishContact() {
|
---|
187 | while (!Serial5.available()) {
|
---|
188 | Serial5.print('A'); // send a capital A
|
---|
189 | delay(300);
|
---|
190 | }
|
---|
191 | Serial.println("Processing Task : Connect!");
|
---|
192 | processing_connected = true;
|
---|
193 | }
|
---|
194 |
|
---|
195 | int inByte = 0;
|
---|
196 | int last_connect = 0;
|
---|
197 |
|
---|
198 | #define TIMEOUT_MS 3000
|
---|
199 |
|
---|
200 | void loop1()
|
---|
201 | {
|
---|
202 | if(!processing_connected){
|
---|
203 | establishContact();
|
---|
204 | }else{
|
---|
205 | if((millis() - last_connect) > TIMEOUT_MS){
|
---|
206 | processing_connected = false;
|
---|
207 | Serial.println("Processing Task : Disconnect!");
|
---|
208 | }
|
---|
209 | }
|
---|
210 |
|
---|
211 | int16_t roll = (int16_t)g_eula_r;
|
---|
212 | int16_t pitch = (int16_t)g_eula_p;
|
---|
213 | int16_t heading;
|
---|
214 |
|
---|
215 | if(g_eula_h <= 180){
|
---|
216 | heading = (int16_t)g_eula_h;
|
---|
217 | }
|
---|
218 | else{
|
---|
219 | heading = (int16_t)(g_eula_h - 360);
|
---|
220 | }
|
---|
221 |
|
---|
222 | if (Serial5.available()){
|
---|
223 | inByte = Serial5.read();
|
---|
224 | last_connect = millis();
|
---|
225 |
|
---|
226 | Serial5.write((uint8_t)(heading >> 8));
|
---|
227 | Serial5.write((uint8_t)heading);
|
---|
228 | Serial5.write((uint8_t)(roll >> 8));
|
---|
229 | Serial5.write((uint8_t)roll);
|
---|
230 | Serial5.write((uint8_t)(pitch >> 8));
|
---|
231 | Serial5.write((uint8_t)pitch);
|
---|
232 | }
|
---|
233 | delay(1);
|
---|
234 | }
|
---|