Janek Mann / openwear-lifelogger-example

Openwear Life logger example

Dependencies:   BLE_API MPL6_1 TCS3472_I2C mbed-src-openwear nRF51822_openwear

Fork of BLE_LoopbackUART by Bluetooth Low Energy

main.cpp@7:9a474d182e4b, 2014-09-08 (annotated)

Committer:
janekm
Date:
Mon Sep 08 23:17:06 2014 +0000
Revision:
7:9a474d182e4b
Parent:
6:68a02e91836e
Child:
8:150a9cb60210
removing AHRS library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yihui 0:e910d9bb040f 1 /* mbed Microcontroller Library
yihui 0:e910d9bb040f 2 * Copyright (c) 2006-2013 ARM Limited
yihui 0:e910d9bb040f 3 *
yihui 0:e910d9bb040f 4 * Licensed under the Apache License, Version 2.0 (the "License");
yihui 0:e910d9bb040f 5 * you may not use this file except in compliance with the License.
yihui 0:e910d9bb040f 6 * You may obtain a copy of the License at
yihui 0:e910d9bb040f 7 *
yihui 0:e910d9bb040f 8 * http://www.apache.org/licenses/LICENSE-2.0
yihui 0:e910d9bb040f 9 *
yihui 0:e910d9bb040f 10 * Unless required by applicable law or agreed to in writing, software
yihui 0:e910d9bb040f 11 * distributed under the License is distributed on an "AS IS" BASIS,
yihui 0:e910d9bb040f 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
yihui 0:e910d9bb040f 13 * See the License for the specific language governing permissions and
yihui 0:e910d9bb040f 14 * limitations under the License.
yihui 0:e910d9bb040f 15 */
yihui 0:e910d9bb040f 16
yihui 0:e910d9bb040f 17 #include "mbed.h"
Rohit Grover 2:e060367b9024 18 #include "BLEDevice.h"
janekm 7:9a474d182e4b 19 #include "MPU9250.h"
janekm 7:9a474d182e4b 20 #include "AHRS.h"
janekm 6:68a02e91836e 21 #include "TCS3472_I2C.h"
yihui 0:e910d9bb040f 22
janekm 6:68a02e91836e 23 I2C i2c(p7, p6);
janekm 7:9a474d182e4b 24 TCS3472_I2C rgb_sensor(&i2c);
janekm 7:9a474d182e4b 25 MPU9250 mpu9250(&i2c);
janekm 7:9a474d182e4b 26
janekm 7:9a474d182e4b 27 Timer t;
yihui 0:e910d9bb040f 28
Rohit Grover 2:e060367b9024 29 BLEDevice ble;
janekm 6:68a02e91836e 30 DigitalOut led1(p27);
janekm 7:9a474d182e4b 31 PwmOut led2(p28);
janekm 6:68a02e91836e 32 DigitalOut LDOOn(p5);
janekm 6:68a02e91836e 33 DigitalOut SoundOn(p2);
janekm 6:68a02e91836e 34 AnalogIn SoundIn(p1);
janekm 6:68a02e91836e 35
janekm 7:9a474d182e4b 36 float sum = 0;
janekm 7:9a474d182e4b 37 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
janekm 7:9a474d182e4b 38 float pitch, yaw, roll;
janekm 7:9a474d182e4b 39 float deltat = 0.0f; // integration interval for both filter schemes
janekm 7:9a474d182e4b 40
janekm 7:9a474d182e4b 41
janekm 7:9a474d182e4b 42 uint32_t sumCount = 0;
janekm 7:9a474d182e4b 43 char buffer[14];
janekm 7:9a474d182e4b 44
janekm 6:68a02e91836e 45 static volatile bool triggerSensorPolling = false;
yihui 0:e910d9bb040f 46
yihui 0:e910d9bb040f 47 // The Nordic UART Service
janekm 6:68a02e91836e 48 static const uint8_t uart_service_uuid[] = {0x6e, 0x40, 0x00, 0x01, 0xb5, 0xa3, 0xf3, 0x93, 0xe0, 0xa9, 0xe5,0x0e, 0x24, 0xdc, 0xca, 0x9e};
Rohit Grover 2:e060367b9024 49 static const uint8_t uart_tx_uuid[] = {0x6e, 0x40, 0x00, 0x02, 0xb5, 0xa3, 0xf3, 0x93, 0xe0, 0xa9, 0xe5,0x0e, 0x24, 0xdc, 0xca, 0x9e};
Rohit Grover 2:e060367b9024 50 static const uint8_t uart_rx_uuid[] = {0x6e, 0x40, 0x00, 0x03, 0xb5, 0xa3, 0xf3, 0x93, 0xe0, 0xa9, 0xe5,0x0e, 0x24, 0xdc, 0xca, 0x9e};
janekm 6:68a02e91836e 51 static const uint8_t uart_service_uuid_rev[] = {0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e};
janekm 6:68a02e91836e 52 static const uint16_t uuid16_list[] = {GattService::UUID_DEVICE_INFORMATION_SERVICE};
rgrover1 5:4bc41267a03a 53
rgrover1 5:4bc41267a03a 54 static const uint8_t SIZEOF_TX_RX_BUFFER = 128;
rgrover1 5:4bc41267a03a 55 uint8_t rxPayload[SIZEOF_TX_RX_BUFFER] = {0,};
rgrover1 5:4bc41267a03a 56 uint8_t txPayload[SIZEOF_TX_RX_BUFFER] = {0,};
janekm 6:68a02e91836e 57
janekm 6:68a02e91836e 58 uint8_t hardwareRevision[] = "0.1";
rgrover1 5:4bc41267a03a 59 GattCharacteristic rxCharacteristic (uart_tx_uuid, rxPayload, 1, SIZEOF_TX_RX_BUFFER,
Rohit Grover 2:e060367b9024 60 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
rgrover1 5:4bc41267a03a 61 GattCharacteristic txCharacteristic (uart_rx_uuid, txPayload, 1, SIZEOF_TX_RX_BUFFER, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
rgrover1 5:4bc41267a03a 62 GattCharacteristic *uartChars[] = {&rxCharacteristic, &txCharacteristic};
janekm 6:68a02e91836e 63 GattService uartService(uart_service_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
janekm 6:68a02e91836e 64
janekm 7:9a474d182e4b 65 GattCharacteristic hardwareRevCharacteristic(GattCharacteristic::UUID_HARDWARE_REVISION_STRING_CHAR, hardwareRevision, sizeof(hardwareRevision), sizeof(hardwareRevision), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
janekm 6:68a02e91836e 66 GattCharacteristic *deviceInfoChars[] = {&hardwareRevCharacteristic};
janekm 6:68a02e91836e 67 GattService deviceInfoService(GattService::UUID_DEVICE_INFORMATION_SERVICE, deviceInfoChars, sizeof(deviceInfoChars) / sizeof(GattCharacteristic *));
yihui 0:e910d9bb040f 68
rgrover1 5:4bc41267a03a 69 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
yihui 0:e910d9bb040f 70 {
Rohit Grover 2:e060367b9024 71 ble.startAdvertising();
Rohit Grover 2:e060367b9024 72 }
yihui 0:e910d9bb040f 73
rgrover1 5:4bc41267a03a 74 void onDataWritten(uint16_t charHandle, const GattCharacteristicWriteCBParams *params)
yihui 0:e910d9bb040f 75 {
rgrover1 5:4bc41267a03a 76 if (charHandle == rxCharacteristic.getValueAttribute().getHandle()) {
rgrover1 5:4bc41267a03a 77 uint16_t bytesRead = params->len;
rgrover1 5:4bc41267a03a 78 if (bytesRead < sizeof(rxPayload)) {
rgrover1 5:4bc41267a03a 79 memcpy(rxPayload, params->data, bytesRead);
rgrover1 5:4bc41267a03a 80 rxPayload[bytesRead] = 0;
rgrover1 5:4bc41267a03a 81 }
rgrover1 5:4bc41267a03a 82 ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), rxPayload, bytesRead);
yihui 0:e910d9bb040f 83 }
Rohit Grover 2:e060367b9024 84 }
yihui 0:e910d9bb040f 85
Rohit Grover 2:e060367b9024 86 void periodicCallback(void)
Rohit Grover 2:e060367b9024 87 {
janekm 6:68a02e91836e 88 triggerSensorPolling = true;
Rohit Grover 2:e060367b9024 89 }
yihui 0:e910d9bb040f 90
yihui 0:e910d9bb040f 91 int main(void)
yihui 0:e910d9bb040f 92 {
Rohit Grover 2:e060367b9024 93 led1 = 1;
janekm 7:9a474d182e4b 94 led2 = 1;
janekm 6:68a02e91836e 95 LDOOn = 1;
janekm 6:68a02e91836e 96 SoundOn = 1;
Rohit Grover 2:e060367b9024 97 Ticker ticker;
janekm 7:9a474d182e4b 98 i2c.frequency(100000);
janekm 7:9a474d182e4b 99 ticker.attach(periodicCallback, 0.3);
janekm 7:9a474d182e4b 100 t.start();
janekm 7:9a474d182e4b 101
janekm 7:9a474d182e4b 102 if (mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250) != 0x71) {
janekm 7:9a474d182e4b 103 while (1);
janekm 7:9a474d182e4b 104 }
janekm 7:9a474d182e4b 105
janekm 7:9a474d182e4b 106 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
janekm 7:9a474d182e4b 107 //led2 = 0;
janekm 7:9a474d182e4b 108 wait(1);
janekm 7:9a474d182e4b 109 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
janekm 7:9a474d182e4b 110 //led1 = 0;
janekm 7:9a474d182e4b 111 mpu9250.calibrateMPU9250(gyroBias, accelBias);
janekm 7:9a474d182e4b 112 mpu9250.getAres(); // Get accelerometer sensitivity
janekm 7:9a474d182e4b 113 mpu9250.getGres(); // Get gyro sensitivity
janekm 7:9a474d182e4b 114 mpu9250.getMres(); // Get magnetometer sensitivity
janekm 7:9a474d182e4b 115 //magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
janekm 7:9a474d182e4b 116 //magbias[1] = +120.; // User environmental x-axis correction in milliGauss
janekm 7:9a474d182e4b 117 //magbias[2] = +125.; // User environmental x-axis correction in milliGauss
janekm 7:9a474d182e4b 118
janekm 7:9a474d182e4b 119 led1 = 0;
janekm 7:9a474d182e4b 120 //while (1);
janekm 7:9a474d182e4b 121 wait(2);
janekm 7:9a474d182e4b 122 mpu9250.initMPU9250();
janekm 7:9a474d182e4b 123 mpu9250.initAK8963(magCalibration);
janekm 7:9a474d182e4b 124 wait(1);
janekm 7:9a474d182e4b 125 led2 = 0;
janekm 7:9a474d182e4b 126
yihui 0:e910d9bb040f 127
Rohit Grover 2:e060367b9024 128 ble.init();
janekm 6:68a02e91836e 129 //rgb_sensor.enableWait();
Rohit Grover 2:e060367b9024 130 ble.onDisconnection(disconnectionCallback);
Rohit Grover 2:e060367b9024 131 ble.onDataWritten(onDataWritten);
yihui 0:e910d9bb040f 132
Rohit Grover 2:e060367b9024 133 /* setup advertising */
Rohit Grover 2:e060367b9024 134 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
Rohit Grover 2:e060367b9024 135 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
Rohit Grover 2:e060367b9024 136 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
janekm 7:9a474d182e4b 137 (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
Rohit Grover 2:e060367b9024 138 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
janekm 7:9a474d182e4b 139 (const uint8_t *)uart_service_uuid_rev, sizeof(uart_service_uuid_rev));
janekm 6:68a02e91836e 140 ble.accumulateScanResponse(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
janekm 6:68a02e91836e 141 //ble.accumulateAdvertisingPayload(GapAdvertisingData::HEART_RATE_SENSOR_HEART_RATE_BELT);
yihui 0:e910d9bb040f 142
Rohit Grover 2:e060367b9024 143 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
janekm 6:68a02e91836e 144 ble.addService(uartService);
janekm 6:68a02e91836e 145 ble.addService(deviceInfoService);
janekm 7:9a474d182e4b 146 rgb_sensor.enablePowerAndRGBC();
janekm 7:9a474d182e4b 147 rgb_sensor.setIntegrationTime( 100 );
janekm 7:9a474d182e4b 148 rgb_sensor.setWaitTime(900);
Rohit Grover 2:e060367b9024 149 ble.startAdvertising();
yihui 0:e910d9bb040f 150
yihui 0:e910d9bb040f 151
Rohit Grover 2:e060367b9024 152 while (true) {
janekm 6:68a02e91836e 153 if (triggerSensorPolling) {
janekm 6:68a02e91836e 154 triggerSensorPolling = false;
janekm 7:9a474d182e4b 155 //led2 = !led2;
janekm 6:68a02e91836e 156 //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
janekm 6:68a02e91836e 157 char reading[20];
janekm 6:68a02e91836e 158 int rgb_readings[4];
janekm 6:68a02e91836e 159 int len;
janekm 6:68a02e91836e 160 rgb_sensor.getAllColors( rgb_readings );
janekm 6:68a02e91836e 161 rgb_sensor.clearInterrupt();
janekm 6:68a02e91836e 162 char whoami;
janekm 6:68a02e91836e 163 float max = 0.0;
janekm 6:68a02e91836e 164 float min = 1.0;
janekm 6:68a02e91836e 165 float current = 0.0;
janekm 6:68a02e91836e 166 for (int i = 0; i < 20; i++) {
janekm 6:68a02e91836e 167 current = SoundIn;
janekm 6:68a02e91836e 168 if (current > max) max = current;
janekm 6:68a02e91836e 169 if (current < min) min = current;
janekm 6:68a02e91836e 170 }
janekm 7:9a474d182e4b 171 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
janekm 7:9a474d182e4b 172 // Now we'll calculate the accleration value into actual g's
janekm 7:9a474d182e4b 173 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
janekm 7:9a474d182e4b 174 ay = (float)accelCount[1]*aRes - accelBias[1];
janekm 7:9a474d182e4b 175 az = (float)accelCount[2]*aRes - accelBias[2];
janekm 6:68a02e91836e 176 len = sprintf((char *)reading, "%d,%d,%d,%d", rgb_readings[0], rgb_readings[1], rgb_readings[2], rgb_readings[3]);
janekm 6:68a02e91836e 177 ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
janekm 6:68a02e91836e 178 len = sprintf((char *)reading, "%1.4f", (max - min) * 10.0);
janekm 6:68a02e91836e 179 ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
janekm 7:9a474d182e4b 180 len = sprintf((char *)reading, "%1.1f, %1.1f, %1.1f", mx, my, mz);
janekm 6:68a02e91836e 181 ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
janekm 6:68a02e91836e 182
janekm 7:9a474d182e4b 183 //whoami = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
janekm 7:9a474d182e4b 184 //len = sprintf((char *)reading, "%d", whoami);
janekm 7:9a474d182e4b 185 //ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
janekm 7:9a474d182e4b 186
janekm 6:68a02e91836e 187 } else {
janekm 7:9a474d182e4b 188 //ble.waitForEvent();
janekm 6:68a02e91836e 189 float max = 0.0;
janekm 6:68a02e91836e 190 float min = 1.0;
janekm 6:68a02e91836e 191 float current = 0.0;
janekm 6:68a02e91836e 192 for (int i = 0; i < 100; i++) {
janekm 6:68a02e91836e 193 current = SoundIn;
janekm 6:68a02e91836e 194 if (current > max) max = current;
janekm 6:68a02e91836e 195 if (current < min) min = current;
janekm 6:68a02e91836e 196 }
janekm 6:68a02e91836e 197 if ((max - min) > 0.005) {
janekm 6:68a02e91836e 198 led2 = 1.0 - (max - min)*5.0;
janekm 6:68a02e91836e 199 if ((max - min) > 0.06) {
janekm 6:68a02e91836e 200 led1 = 0;
janekm 6:68a02e91836e 201 } else {
janekm 6:68a02e91836e 202 led1 = 1;
janekm 6:68a02e91836e 203 }
janekm 6:68a02e91836e 204 } else {
janekm 6:68a02e91836e 205 led2 = 1.0;
janekm 6:68a02e91836e 206 led1 = 1;
janekm 6:68a02e91836e 207 }
janekm 7:9a474d182e4b 208
janekm 7:9a474d182e4b 209 // If intPin goes high, all data registers have new data
janekm 7:9a474d182e4b 210 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
janekm 7:9a474d182e4b 211 //led1 = !led1;
janekm 7:9a474d182e4b 212 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
janekm 7:9a474d182e4b 213 // Now we'll calculate the accleration value into actual g's
janekm 7:9a474d182e4b 214 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
janekm 7:9a474d182e4b 215 ay = (float)accelCount[1]*aRes - accelBias[1];
janekm 7:9a474d182e4b 216 az = (float)accelCount[2]*aRes - accelBias[2];
janekm 7:9a474d182e4b 217
janekm 7:9a474d182e4b 218 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
janekm 7:9a474d182e4b 219 // Calculate the gyro value into actual degrees per second
janekm 7:9a474d182e4b 220 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
janekm 7:9a474d182e4b 221 gy = (float)gyroCount[1]*gRes - gyroBias[1];
janekm 7:9a474d182e4b 222 gz = (float)gyroCount[2]*gRes - gyroBias[2];
janekm 7:9a474d182e4b 223
janekm 7:9a474d182e4b 224 mpu9250.readMagData(magCount); // Read the x/y/z adc values
janekm 7:9a474d182e4b 225 // Calculate the magnetometer values in milliGauss
janekm 7:9a474d182e4b 226 // Include factory calibration per data sheet and user environmental corrections
janekm 7:9a474d182e4b 227 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
janekm 7:9a474d182e4b 228 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
janekm 7:9a474d182e4b 229 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
janekm 7:9a474d182e4b 230
janekm 7:9a474d182e4b 231
janekm 7:9a474d182e4b 232 Now = t.read_us();
janekm 7:9a474d182e4b 233 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
janekm 7:9a474d182e4b 234 lastUpdate = Now;
janekm 7:9a474d182e4b 235
janekm 7:9a474d182e4b 236 sum += deltat;
janekm 7:9a474d182e4b 237 sumCount++;
janekm 7:9a474d182e4b 238
janekm 7:9a474d182e4b 239 MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz, deltat, q);
janekm 7:9a474d182e4b 240
janekm 7:9a474d182e4b 241 // Serial print and/or display at 0.5 s rate independent of data rates
janekm 7:9a474d182e4b 242 delt_t = t.read_ms() - count;
janekm 7:9a474d182e4b 243 if (delt_t > 500) { // update LCD once per half-second independent of read rate
janekm 7:9a474d182e4b 244
janekm 7:9a474d182e4b 245
janekm 7:9a474d182e4b 246 tempCount = mpu9250.readTempData(); // Read the adc values
janekm 7:9a474d182e4b 247 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
janekm 7:9a474d182e4b 248
janekm 7:9a474d182e4b 249 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
janekm 7:9a474d182e4b 250 // In this coordinate system, the positive z-axis is down toward Earth.
janekm 7:9a474d182e4b 251 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
janekm 7:9a474d182e4b 252 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
janekm 7:9a474d182e4b 253 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
janekm 7:9a474d182e4b 254 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
janekm 7:9a474d182e4b 255 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
janekm 7:9a474d182e4b 256 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
janekm 7:9a474d182e4b 257 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
janekm 7:9a474d182e4b 258 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
janekm 7:9a474d182e4b 259 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
janekm 7:9a474d182e4b 260 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
janekm 7:9a474d182e4b 261 pitch *= 180.0f / PI;
janekm 7:9a474d182e4b 262 yaw *= 180.0f / PI;
janekm 7:9a474d182e4b 263 // yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
janekm 7:9a474d182e4b 264 roll *= 180.0f / PI;
janekm 7:9a474d182e4b 265
janekm 7:9a474d182e4b 266 count = t.read_ms();
janekm 7:9a474d182e4b 267
janekm 7:9a474d182e4b 268 if(count > 1<<21) {
janekm 7:9a474d182e4b 269 t.start(); // start the timer over again if ~30 minutes has passed
janekm 7:9a474d182e4b 270 count = 0;
janekm 7:9a474d182e4b 271 deltat= 0;
janekm 7:9a474d182e4b 272 lastUpdate = t.read_us();
janekm 7:9a474d182e4b 273 }
janekm 7:9a474d182e4b 274 sum = 0;
janekm 7:9a474d182e4b 275 sumCount = 0;
janekm 7:9a474d182e4b 276 }
janekm 7:9a474d182e4b 277 }
janekm 6:68a02e91836e 278 }
yihui 0:e910d9bb040f 279 }
yihui 0:e910d9bb040f 280 }