Janek Mann
Openwear Life logger example
Dependencies: BLE_API MPL6_1 TCS3472_I2C mbed-src-openwear nRF51822_openwear
Fork of
BLE_LoopbackUART
by 
Bluetooth Low Energy
Revision 7:9a474d182e4b, committed 2014-09-08
- Comitter:
- janekm
- Date:
- Mon Sep 08 23:17:06 2014 +0000
- Parent:
- 6:68a02e91836e
- Child:
- 8:150a9cb60210
- Commit message:
- removing AHRS library
Changed in this revision
--- a/BLE_API.lib Thu Sep 04 21:19:18 2014 +0000 +++ b/BLE_API.lib Mon Sep 08 23:17:06 2014 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/teams/Bluetooth-Low-Energy/code/BLE_API/#ca826083980e +http://mbed.org/teams/Bluetooth-Low-Energy/code/BLE_API/#0fb20195102b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPL6_1.lib Mon Sep 08 23:17:06 2014 +0000 @@ -0,0 +1,1 @@ +MPL6_1#61a7cadab29e
--- a/MPU9250AHRS.lib Thu Sep 04 21:19:18 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/janekm/code/MPU9250AHRS/#404c35f32ce3
--- a/TCS3472_I2C.lib Thu Sep 04 21:19:18 2014 +0000 +++ b/TCS3472_I2C.lib Mon Sep 08 23:17:06 2014 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/karlmaxwell67/code/TCS3472_I2C/#fbc4c6f3be5b +http://mbed.org/users/karlmaxwell67/code/TCS3472_I2C/#2a240f6ca27a
--- a/main.cpp Thu Sep 04 21:19:18 2014 +0000
+++ b/main.cpp Mon Sep 08 23:17:06 2014 +0000
@@ -16,18 +16,32 @@
#include "mbed.h"
#include "BLEDevice.h"
+#include "MPU9250.h"
+#include "AHRS.h"
#include "TCS3472_I2C.h"
I2C i2c(p7, p6);
-TCS3472_I2C rgb_sensor( &i2c );
+TCS3472_I2C rgb_sensor(&i2c);
+MPU9250 mpu9250(&i2c);
+
+Timer t;
BLEDevice ble;
DigitalOut led1(p27);
-PwmOut led2(p28);
+PwmOut led2(p28);
DigitalOut LDOOn(p5);
DigitalOut SoundOn(p2);
AnalogIn SoundIn(p1);
+float sum = 0;
+float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
+float pitch, yaw, roll;
+float deltat = 0.0f; // integration interval for both filter schemes
+
+
+uint32_t sumCount = 0;
+char buffer[14];
+
static volatile bool triggerSensorPolling = false;
// The Nordic UART Service
@@ -48,7 +62,7 @@
GattCharacteristic *uartChars[] = {&rxCharacteristic, &txCharacteristic};
GattService uartService(uart_service_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
-GattCharacteristic hardwareRevCharacteristic(GattCharacteristic::UUID_HARDWARE_REVISION_STRING_CHAR, hardwareRevision, sizeof(hardwareRevision), sizeof(hardwareRevision), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
+GattCharacteristic hardwareRevCharacteristic(GattCharacteristic::UUID_HARDWARE_REVISION_STRING_CHAR, hardwareRevision, sizeof(hardwareRevision), sizeof(hardwareRevision), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ);
GattCharacteristic *deviceInfoChars[] = {&hardwareRevCharacteristic};
GattService deviceInfoService(GattService::UUID_DEVICE_INFORMATION_SERVICE, deviceInfoChars, sizeof(deviceInfoChars) / sizeof(GattCharacteristic *));
@@ -77,17 +91,41 @@
int main(void)
{
led1 = 1;
- led2 = 1.0;
+ led2 = 1;
LDOOn = 1;
SoundOn = 1;
Ticker ticker;
- i2c.frequency(400000);
- ticker.attach(periodicCallback, 4);
+ i2c.frequency(100000);
+ ticker.attach(periodicCallback, 0.3);
+ t.start();
+
+ if (mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250) != 0x71) {
+ while (1);
+ }
+
+ mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+ //led2 = 0;
+ wait(1);
+ mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
+ //led1 = 0;
+ mpu9250.calibrateMPU9250(gyroBias, accelBias);
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ //magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
+ //magbias[1] = +120.; // User environmental x-axis correction in milliGauss
+ //magbias[2] = +125.; // User environmental x-axis correction in milliGauss
+
+ led1 = 0;
+ //while (1);
+ wait(2);
+ mpu9250.initMPU9250();
+ mpu9250.initAK8963(magCalibration);
+ wait(1);
+ led2 = 0;
+
ble.init();
- rgb_sensor.enablePowerAndRGBC();
- rgb_sensor.setIntegrationTime( 100 );
- rgb_sensor.setWaitTime(900);
//rgb_sensor.enableWait();
ble.onDisconnection(disconnectionCallback);
ble.onDataWritten(onDataWritten);
@@ -96,21 +134,25 @@
ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
- (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
+ (const uint8_t *)"BLE UART", sizeof("BLE UART") - 1);
ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
- (const uint8_t *)uart_service_uuid_rev, sizeof(uart_service_uuid_rev));
+ (const uint8_t *)uart_service_uuid_rev, sizeof(uart_service_uuid_rev));
ble.accumulateScanResponse(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
//ble.accumulateAdvertisingPayload(GapAdvertisingData::HEART_RATE_SENSOR_HEART_RATE_BELT);
ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
ble.addService(uartService);
ble.addService(deviceInfoService);
+ rgb_sensor.enablePowerAndRGBC();
+ rgb_sensor.setIntegrationTime( 100 );
+ rgb_sensor.setWaitTime(900);
ble.startAdvertising();
while (true) {
if (triggerSensorPolling) {
triggerSensorPolling = false;
+ //led2 = !led2;
//led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
char reading[20];
int rgb_readings[4];
@@ -126,16 +168,24 @@
if (current > max) max = current;
if (current < min) min = current;
}
+ mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
+ // Now we'll calculate the accleration value into actual g's
+ ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2];
len = sprintf((char *)reading, "%d,%d,%d,%d", rgb_readings[0], rgb_readings[1], rgb_readings[2], rgb_readings[3]);
ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
len = sprintf((char *)reading, "%1.4f", (max - min) * 10.0);
ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
- whoami = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
- len = sprintf((char *)reading, "%d", whoami);
+ len = sprintf((char *)reading, "%1.1f, %1.1f, %1.1f", mx, my, mz);
ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
+ //whoami = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
+ //len = sprintf((char *)reading, "%d", whoami);
+ //ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(),(uint8_t *) reading, len);
+
} else {
- //ble.waitForEvent();
+ //ble.waitForEvent();
float max = 0.0;
float min = 1.0;
float current = 0.0;
@@ -155,6 +205,76 @@
led2 = 1.0;
led1 = 1;
}
+
+ // If intPin goes high, all data registers have new data
+ if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
+ //led1 = !led1;
+ mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
+ // Now we'll calculate the accleration value into actual g's
+ ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2];
+
+ mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
+ // Calculate the gyro value into actual degrees per second
+ gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
+ gy = (float)gyroCount[1]*gRes - gyroBias[1];
+ gz = (float)gyroCount[2]*gRes - gyroBias[2];
+
+ mpu9250.readMagData(magCount); // Read the x/y/z adc values
+ // Calculate the magnetometer values in milliGauss
+ // Include factory calibration per data sheet and user environmental corrections
+ mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
+ my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+ mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+
+
+ Now = t.read_us();
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+
+ sum += deltat;
+ sumCount++;
+
+ MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz, deltat, q);
+
+ // Serial print and/or display at 0.5 s rate independent of data rates
+ delt_t = t.read_ms() - count;
+ if (delt_t > 500) { // update LCD once per half-second independent of read rate
+
+
+ tempCount = mpu9250.readTempData(); // Read the adc values
+ temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
+
+ // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+ // In this coordinate system, the positive z-axis is down toward Earth.
+ // 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.
+ // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+ // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+ // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+ // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+ // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+ // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+ 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]);
+ pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ 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]);
+ pitch *= 180.0f / PI;
+ yaw *= 180.0f / PI;
+ // yaw -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
+ roll *= 180.0f / PI;
+
+ count = t.read_ms();
+
+ if(count > 1<<21) {
+ t.start(); // start the timer over again if ~30 minutes has passed
+ count = 0;
+ deltat= 0;
+ lastUpdate = t.read_us();
+ }
+ sum = 0;
+ sumCount = 0;
+ }
+ }
}
}
}
--- a/mbed-src-openwear.lib Thu Sep 04 21:19:18 2014 +0000 +++ b/mbed-src-openwear.lib Mon Sep 08 23:17:06 2014 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/janekm/code/mbed-src-openwear/#61b24bcb4679 +http://mbed.org/users/janekm/code/mbed-src-openwear/#1576dac518fb