File content as of revision 7:9a474d182e4b:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2013 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#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);
MPU9250 mpu9250(&i2c);

Timer t;

BLEDevice  ble;
DigitalOut led1(p27);
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
static const uint8_t uart_service_uuid[] = {0x6e, 0x40, 0x00, 0x01, 0xb5, 0xa3, 0xf3, 0x93, 0xe0, 0xa9, 0xe5,0x0e, 0x24, 0xdc, 0xca, 0x9e};
static const uint8_t uart_tx_uuid[]   = {0x6e, 0x40, 0x00, 0x02, 0xb5, 0xa3, 0xf3, 0x93, 0xe0, 0xa9, 0xe5,0x0e, 0x24, 0xdc, 0xca, 0x9e};
static const uint8_t uart_rx_uuid[]   = {0x6e, 0x40, 0x00, 0x03, 0xb5, 0xa3, 0xf3, 0x93, 0xe0, 0xa9, 0xe5,0x0e, 0x24, 0xdc, 0xca, 0x9e};
static const uint8_t uart_service_uuid_rev[] = {0x9e, 0xca, 0xdc, 0x24, 0x0e, 0xe5, 0xa9, 0xe0, 0x93, 0xf3, 0xa3, 0xb5, 0x01, 0x00, 0x40, 0x6e};
static const uint16_t uuid16_list[]        = {GattService::UUID_DEVICE_INFORMATION_SERVICE};

static const uint8_t SIZEOF_TX_RX_BUFFER = 128;
uint8_t rxPayload[SIZEOF_TX_RX_BUFFER] = {0,};
uint8_t txPayload[SIZEOF_TX_RX_BUFFER] = {0,};

uint8_t hardwareRevision[] = "0.1";
GattCharacteristic  rxCharacteristic (uart_tx_uuid, rxPayload, 1, SIZEOF_TX_RX_BUFFER,
                                      GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic  txCharacteristic (uart_rx_uuid, txPayload, 1, SIZEOF_TX_RX_BUFFER, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
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  *deviceInfoChars[] = {&hardwareRevCharacteristic};
GattService         deviceInfoService(GattService::UUID_DEVICE_INFORMATION_SERVICE, deviceInfoChars, sizeof(deviceInfoChars) / sizeof(GattCharacteristic *));

void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    ble.startAdvertising();
}

void onDataWritten(uint16_t charHandle, const GattCharacteristicWriteCBParams *params)
{
    if (charHandle == rxCharacteristic.getValueAttribute().getHandle()) {
        uint16_t bytesRead = params->len;
        if (bytesRead < sizeof(rxPayload)) {
            memcpy(rxPayload, params->data, bytesRead);
            rxPayload[bytesRead] = 0;
        }
        ble.updateCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), rxPayload, bytesRead);
    }
}

void periodicCallback(void)
{
    triggerSensorPolling = true;
}

int main(void)
{
    led1 = 1;
    led2 = 1;
    LDOOn = 1;
    SoundOn = 1;
    Ticker ticker;
    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.enableWait();
    ble.onDisconnection(disconnectionCallback);
    ble.onDataWritten(onDataWritten);

    /* setup advertising */
    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);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
                                     (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];
            int len;
            rgb_sensor.getAllColors( rgb_readings );
            rgb_sensor.clearInterrupt();
            char whoami;
            float max = 0.0;
            float min = 1.0;
            float current = 0.0;
            for (int i = 0; i < 20; i++) {
                current = SoundIn;
                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);
            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();
            float max = 0.0;
            float min = 1.0;
            float current = 0.0;
            for (int i = 0; i < 100; i++) {
                current = SoundIn;
                if (current > max) max = current;
                if (current < min) min = current;
            }
            if ((max - min) > 0.005) {
                led2 = 1.0 - (max - min)*5.0;
                if ((max - min) > 0.06) {
                    led1 = 0;
                } else {
                    led1 = 1;
                }
            } else {
                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;
                }
            }
        }
    }
}