File content as of revision 1:28c107f8cf24:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2014 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 <events/mbed_events.h>
#include <mbed.h>
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "pretty_printer.h"

/***
Library used by PCA

#include "mbed.h"
#include "platform/mbed_thread.h"
#include "stats_report.h"
#include "MPU9250.h"
#include <Eigen/Dense.h>
#include <iostream>
***/

/***
Fields used by PCA

using namespace std;
using namespace Eigen;

DigitalOut led1(LED1);
const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0

#define SLEEP_TIME 5000 // (msec)
***/

/***
Methods used by PCA

///*
//  * Normalize the Matrix X
//  */
// MatrixXd featurnormail(MatrixXd &X)
//{
//    //I don't know why to use the transpose
//    //compute the mean of every dimension
//    MatrixXd X1 = X.transpose();
//    MatrixXd meanval = X1.colwise().mean();
//    
//    //normalization
//    RowVectorXd meanvecRow = meanval;
//    X1.rowwise() -= meanvecRow;
//    
//    return X1.transpose();
//}
//
// /*
//  * Compute the Covariane Matrix of X, put to C
//  * C = 1/m * X * X.transpose 
//  */
//void ComComputeCov(MatrixXd &X, MatrixXd &C)
//{
//   
//    C = X*X.adjoint();//same as XT*X a
//    //translate to array
//    C = C.array() / X.cols();
//}
// 
// 
///*
// * Compute the eigenvalue and eigenvector of C
// * val = (first eigenvalue) --smallest --not important
// *              .
// *              .
// *              .
// *       (last eigenvalue)  --largest -- important
// *
// * vec = (first eigenvector, ... , last eigenvector)
// *           not important          important
// */
//void ComputEig(MatrixXd &C, MatrixXd &vec, MatrixXd &val)
//{
//    //SelfAdjointEigenSolver will sort the values automatically
//    SelfAdjointEigenSolver<MatrixXd> eig(C);
//    vec = eig.eigenvectors();
//    val = eig.eigenvalues();
//}
// 
///* Compute the dimension need to include enough information of raw data.
// * form large index to small index, since the val is sorted from small to large.
// * in some cases, just decide the number of dimension, instead of compute it.
// */
//int ComputDim(MatrixXd &val)
//{
//    int dim;
//    double sum = 0;
//    for (int i = val.rows() - 1; i >= 0;--i)
//    {
//        sum += val(i, 0);
//        dim = i;
//        if (sum / val.sum()>=0.8)//80% of the information
//            break;
//    }
//    return val.rows() - dim;
//}
***/

static DigitalOut led1(LED1, 1);

const static char DEVICE_NAME[] = "STEP COUNTER";

const static uint16_t STEP_COUNTER_SERVICE_UUID = 0xA000;
const static uint16_t STEP_COUNTER_CHARACTERISTIC_UUID = 0xA001;

int step_count = 0;
int id = 0;
ReadWriteGattCharacteristic<int> step_count_state(STEP_COUNTER_CHARACTERISTIC_UUID, &step_count, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);

static events::EventQueue event_queue(/* event count */ 16 * EVENTS_EVENT_SIZE);

class StepCounter : ble::Gap::EventHandler {
public:
    StepCounter(BLE &ble, events::EventQueue &event_queue) :
        _ble(ble),
        _event_queue(event_queue),
        _step_counter_uuid(STEP_COUNTER_SERVICE_UUID),
        _adv_data_builder(_adv_buffer) { }

    void start() {
        _ble.gap().setEventHandler(this);

        _ble.init(this, &StepCounter::on_init_complete);
        _event_queue.call_every(500, this, &StepCounter::blink);
        
        // TODO Replace this to sync with data reading in order to update step
        // counts by exact window.
        _event_queue.call_every(1000, this, &StepCounter::update_step_count);

        _event_queue.dispatch_forever();
    }

private:
    /** Callback triggered when the ble initialization process has finished */
    void on_init_complete(BLE::InitializationCompleteCallbackContext *params) {
        if (params->error != BLE_ERROR_NONE) {
            print_error(params->error, "Ble initialization failed.");
            return;
        }
        
        _ble.gattServer().onDataWritten(this, &StepCounter::on_data_written);

        print_mac_address();

        start_advertising();
    }

    void start_advertising() {
        /* Create advertising parameters and payload */

        ble::AdvertisingParameters adv_parameters(
            ble::advertising_type_t::CONNECTABLE_UNDIRECTED,
            ble::adv_interval_t(ble::millisecond_t(1000))
        );

        _adv_data_builder.setFlags();
        _adv_data_builder.setLocalServiceList(mbed::make_Span(&_step_counter_uuid, 1));
        _adv_data_builder.setName(DEVICE_NAME);

        /* Setup advertising */

        ble_error_t error = _ble.gap().setAdvertisingParameters(
            ble::LEGACY_ADVERTISING_HANDLE,
            adv_parameters
        );

        if (error) {
            print_error(error, "_ble.gap().setAdvertisingParameters() failed");
            return;
        }

        error = _ble.gap().setAdvertisingPayload(
            ble::LEGACY_ADVERTISING_HANDLE,
            _adv_data_builder.getAdvertisingData()
        );

        if (error) {
            print_error(error, "_ble.gap().setAdvertisingPayload() failed");
            return;
        }

        /* Start advertising */

        error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);

        if (error) {
            print_error(error, "_ble.gap().startAdvertising() failed");
            return;
        }
    }
    
    void on_data_written(const GattWriteCallbackParams *params) {
        if ((params->handle == step_count_state.getValueHandle()) && (params->len == 1)) {
            step_count = *(params->data);
        }
        step_count = 0;
    }

    void update_step_count() {
        if (_ble.gap().getState().connected) {
            // TODO Remove step_count increament which would be implemented at 
            // main() after peak detection.
            step_count++;

            _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int));
        }
    }

    void blink(void) {
        led1 = !led1;
    }

private:
    /* Event handler */

    void onDisconnectionComplete(const ble::DisconnectionCompleteEvent&) {
        _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
    }

private:
    BLE &_ble;
    events::EventQueue &_event_queue;
    
    UUID _step_counter_uuid;

    uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE];
    ble::AdvertisingDataBuilder _adv_data_builder;
};

/** Schedule processing of events from the BLE middleware in the event queue. */
void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) {
    event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents));
}

int main()
{
    BLE &ble = BLE::Instance();
    
    GattCharacteristic *charTable[] = {&step_count_state};
    GattService step_count_service(STEP_COUNTER_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
    ble.addService(step_count_service);
    
    ble.onEventsToProcess(schedule_ble_events);

    StepCounter demo(ble, event_queue);
    demo.start();
    
    // read data
    /***
    PCA parameters init
    
        //new mpu(data,clk,address),in constructor addr7bit<<1
    mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
    //scale of acc and gyro
    mpu->initMPU9250(0x00,0x00);

    float AccRead[3];
    float GyroRead[3];
    float TempRead[1];
    
    
    MatrixXd acc_raw(3,0);
    Vector3d acc_new;
    MatrixXd C;
    MatrixXd vec, val;
    int dim = 1;    //dimension of PCA
    ***/
    /***
    Reading data and do PCA
    
    while (true) {
        
        //Blink LED and wait 1 seconds
        // TODO Question actually: Is this to wait for collecting enough data
        // for a window?
        led1 = !led1;
        thread_sleep_for(SLEEP_TIME);

        //read and convert date
        mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
        //printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
        //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
        //printf("temp value is %f.\n\r",TempRead[0]);
        
        //append new data to matrix acc_raw
        //adding the columns
        acc_new << AccRead[0],AccRead[1],AccRead[2];
        acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
        acc_raw.col(acc_raw.cols()-1) = acc_new;
        //cout << "acc_raw:" << acc_raw << endl;
        
        // TODO Check if there are enough data for a single window
        // if true -> run PCA and peak detection
        // else    -> sleep for another 1 second maybe?
        
        //run PCA
        MatrixXd X1=featurnormail(acc_raw);
        ComComputeCov(X1, C);
        ComputEig(C, vec, val);
        //select dim num of eigenvector from right to left. right is important
        //compute the result array
        MatrixXd res = vec.rightCols(dim).transpose()*X1;
        
        //show the result after PCA
        //cout << "result" << res << endl;
        
        // TODO add peak detection algorithm here
        
        // TODO add step count.  Also, think about lock of the variable - what
        // if 
    }
    ***/
    

    return 0;
}