[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / examples / AttitudeDisplay / imuLoop.ino
1 /*
2 * ======== imuLoop ========
3 * This sketch intializes the direction cosine matrix state with initial
4 * sensor readings, then continuously reads the sensors at 200 Hz and
5 * updates the DCM based on the sensor data.
6 *
7 * In addition, this sketch implements the magnetometer calibration sequence
8 * which normalizes the raw magnetometer data for use in DCM calculations. During
9 * setup, an LED will light up, indicating that the calibration sequence is running.
10 * rotating the Zumo sufficiently will complete the calibration and the LED will
11 * turn off.
12 */
14 #include <Wire.h>
15 #include <Energia.h>
17 #include <DCM.h>
18 #include <IMUManager.h>
20 /* IMU data instance objects */
21 IMUManager imu;
22 DCM_Handle imuDCM;
24 static void calibrateDCM(DCM_Handle dcm);
25 static void getIMUData(DCM_Handle dcm);
27 /*
28 * ======== imuSetup ========
29 */
30 void imuSetup()
31 {
32 Serial.begin(9600);
33 Serial.println("imuSetup ...");
35 Wire.begin();
37 imu = IMUManager();
39 /* initialize Zumo accelerometer and magnetometer */
40 imu.initAccel();
41 imu.enableAccelDefault();
43 /* initialize Zumo gyro */
44 if (!imu.initGyro()) {
45 Serial.print("Failed to autodetect gyro type!");
46 delay(1000);
47 }
48 imu.enableGyroDefault();
50 /* calibrate the gyro */
51 Serial.println("Calibrating gyro for 2 seconds: keep zumo still during calibration period");
52 imu.calibrateGyro(2);
53 imu.zeroGyroZAxis();
54 imu.zeroGyroYAxis();
55 imu.zeroGyroZAxis();
57 /* create a new DCM 'object' with
58 * update time 5 ms, and scaling constants (0.03, 0.97, 0.001)
59 */
60 imuDCM = DCM_create(0.005f, 0.03f, 0.97f, 0.01f);
61 }
63 /*
64 * ======== imuLoop ========
65 */
66 void imuLoop()
67 {
68 /* calibrate DCM filters */
69 calibrateDCM(imuDCM);
71 /* update imuDCM's IMU data every 5 ms (200 Hz) */
72 getIMUData(imuDCM);
74 /* re-compute imuDCM's values */
75 DCM_update(imuDCM);
77 delay(5);
78 }
80 /*
81 * ======== calibrateDCM ========
82 * Calibrate the magnetometer and initialize the DCM object (if necessary)
83 */
84 static void calibrateDCM(DCM_Handle dcm)
85 {
86 static bool calibrated = 0;
87 if (!calibrated) {
88 calibrated = 1;
90 /* calibrate magnetometer */
91 Serial.print("Calibrating magnetometer: rotate the zumo along all axes during calibration period ... ");
93 /* illuminate LED while calibrating */
94 digitalWrite(MAIN_LED_PIN, HIGH);
96 imu.calibrateMagnetometer(200);
97 Serial.println("done.");
99 /* shutoff LED */
100 digitalWrite(MAIN_LED_PIN, LOW);
102 /* get initial calibrated IMU data for dcm */
103 getIMUData(dcm);
105 /* reset the dcm's filters starting from initial estimates */
106 DCM_start(dcm);
107 }
108 }
110 /*
111 * ======== getIMUData ========
112 * Provide a new reading of the IMU data to the DCM object
113 */
114 static void getIMUData(DCM_Handle dcm)
115 {
116 float gyroX, gyroY, gyroZ;
118 /* read data from the sensors */
119 imu.readGyro();
120 imu.readAccel();
121 imu.readMag();
123 /* convert gyro angular velocity from deg/s to rad/s */
124 gyroX = 0.0174532925f * (imu.getGyroX());
125 gyroY = 0.0174532925f * (imu.getGyroY());
126 gyroZ = 0.0174532925f * (imu.getGyroZ());
128 /* push initial IMU data into the DCM "filter" */
129 DCM_updateAccelData(dcm, imu.getAccelX(), imu.getAccelY(), imu.getAccelZ());
130 DCM_updateGyroData(dcm, gyroX, gyroY, gyroZ);
131 DCM_updateMagnetoData(dcm, imu.getMagX(), imu.getMagY(), imu.getMagZ());
132 }