Openwear Life logger example
Dependencies: BLE_API MPL6_1 TCS3472_I2C mbed-src-openwear nRF51822_openwear
Fork of BLE_LoopbackUART by
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?
User | Revision | Line number | New 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 | } |