Shiyao Zhang / Mbed OS step_counter_service_ble

add codes for data reading and PCA

Dependencies:   mpu9250_i2c Eigen

source/main.cpp@1:28c107f8cf24, 2019-11-20 (annotated)

Committer:
shiyaozhanag
Date:
Wed Nov 20 00:55:20 2019 +0000
Revision:
1:28c107f8cf24
Parent:
0:dbcc46819a5e 
add data reading and PCA as comments

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nikoletakuneva 0:dbcc46819a5e 1 /* mbed Microcontroller Library
nikoletakuneva 0:dbcc46819a5e 2 * Copyright (c) 2006-2014 ARM Limited
nikoletakuneva 0:dbcc46819a5e 3 *
nikoletakuneva 0:dbcc46819a5e 4 * Licensed under the Apache License, Version 2.0 (the "License");
nikoletakuneva 0:dbcc46819a5e 5 * you may not use this file except in compliance with the License.
nikoletakuneva 0:dbcc46819a5e 6 * You may obtain a copy of the License at
nikoletakuneva 0:dbcc46819a5e 7 *
nikoletakuneva 0:dbcc46819a5e 8 * http://www.apache.org/licenses/LICENSE-2.0
nikoletakuneva 0:dbcc46819a5e 9 *
nikoletakuneva 0:dbcc46819a5e 10 * Unless required by applicable law or agreed to in writing, software
nikoletakuneva 0:dbcc46819a5e 11 * distributed under the License is distributed on an "AS IS" BASIS,
nikoletakuneva 0:dbcc46819a5e 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
nikoletakuneva 0:dbcc46819a5e 13 * See the License for the specific language governing permissions and
nikoletakuneva 0:dbcc46819a5e 14 * limitations under the License.
nikoletakuneva 0:dbcc46819a5e 15 */
nikoletakuneva 0:dbcc46819a5e 16
nikoletakuneva 0:dbcc46819a5e 17 #include <events/mbed_events.h>
nikoletakuneva 0:dbcc46819a5e 18 #include <mbed.h>
nikoletakuneva 0:dbcc46819a5e 19 #include "ble/BLE.h"
nikoletakuneva 0:dbcc46819a5e 20 #include "ble/Gap.h"
nikoletakuneva 0:dbcc46819a5e 21 #include "pretty_printer.h"
nikoletakuneva 0:dbcc46819a5e 22
shiyaozhanag 1:28c107f8cf24 23 /***
shiyaozhanag 1:28c107f8cf24 24 Library used by PCA
shiyaozhanag 1:28c107f8cf24 25
shiyaozhanag 1:28c107f8cf24 26 #include "mbed.h"
shiyaozhanag 1:28c107f8cf24 27 #include "platform/mbed_thread.h"
shiyaozhanag 1:28c107f8cf24 28 #include "stats_report.h"
shiyaozhanag 1:28c107f8cf24 29 #include "MPU9250.h"
shiyaozhanag 1:28c107f8cf24 30 #include <Eigen/Dense.h>
shiyaozhanag 1:28c107f8cf24 31 #include <iostream>
shiyaozhanag 1:28c107f8cf24 32 ***/
shiyaozhanag 1:28c107f8cf24 33
shiyaozhanag 1:28c107f8cf24 34 /***
shiyaozhanag 1:28c107f8cf24 35 Fields used by PCA
shiyaozhanag 1:28c107f8cf24 36
shiyaozhanag 1:28c107f8cf24 37 using namespace std;
shiyaozhanag 1:28c107f8cf24 38 using namespace Eigen;
shiyaozhanag 1:28c107f8cf24 39
shiyaozhanag 1:28c107f8cf24 40 DigitalOut led1(LED1);
shiyaozhanag 1:28c107f8cf24 41 const int addr7bit = 0x68; // 7bit I2C address,AD0 is 0
shiyaozhanag 1:28c107f8cf24 42
shiyaozhanag 1:28c107f8cf24 43 #define SLEEP_TIME 5000 // (msec)
shiyaozhanag 1:28c107f8cf24 44 ***/
shiyaozhanag 1:28c107f8cf24 45
shiyaozhanag 1:28c107f8cf24 46 /***
shiyaozhanag 1:28c107f8cf24 47 Methods used by PCA
shiyaozhanag 1:28c107f8cf24 48
shiyaozhanag 1:28c107f8cf24 49 ///*
shiyaozhanag 1:28c107f8cf24 50 // * Normalize the Matrix X
shiyaozhanag 1:28c107f8cf24 51 // */
shiyaozhanag 1:28c107f8cf24 52 // MatrixXd featurnormail(MatrixXd &X)
shiyaozhanag 1:28c107f8cf24 53 //{
shiyaozhanag 1:28c107f8cf24 54 // //I don't know why to use the transpose
shiyaozhanag 1:28c107f8cf24 55 // //compute the mean of every dimension
shiyaozhanag 1:28c107f8cf24 56 // MatrixXd X1 = X.transpose();
shiyaozhanag 1:28c107f8cf24 57 // MatrixXd meanval = X1.colwise().mean();
shiyaozhanag 1:28c107f8cf24 58 //
shiyaozhanag 1:28c107f8cf24 59 // //normalization
shiyaozhanag 1:28c107f8cf24 60 // RowVectorXd meanvecRow = meanval;
shiyaozhanag 1:28c107f8cf24 61 // X1.rowwise() -= meanvecRow;
shiyaozhanag 1:28c107f8cf24 62 //
shiyaozhanag 1:28c107f8cf24 63 // return X1.transpose();
shiyaozhanag 1:28c107f8cf24 64 //}
shiyaozhanag 1:28c107f8cf24 65 //
shiyaozhanag 1:28c107f8cf24 66 // /*
shiyaozhanag 1:28c107f8cf24 67 // * Compute the Covariane Matrix of X, put to C
shiyaozhanag 1:28c107f8cf24 68 // * C = 1/m * X * X.transpose
shiyaozhanag 1:28c107f8cf24 69 // */
shiyaozhanag 1:28c107f8cf24 70 //void ComComputeCov(MatrixXd &X, MatrixXd &C)
shiyaozhanag 1:28c107f8cf24 71 //{
shiyaozhanag 1:28c107f8cf24 72 //
shiyaozhanag 1:28c107f8cf24 73 // C = X*X.adjoint();//same as XT*X a
shiyaozhanag 1:28c107f8cf24 74 // //translate to array
shiyaozhanag 1:28c107f8cf24 75 // C = C.array() / X.cols();
shiyaozhanag 1:28c107f8cf24 76 //}
shiyaozhanag 1:28c107f8cf24 77 //
shiyaozhanag 1:28c107f8cf24 78 //
shiyaozhanag 1:28c107f8cf24 79 ///*
shiyaozhanag 1:28c107f8cf24 80 // * Compute the eigenvalue and eigenvector of C
shiyaozhanag 1:28c107f8cf24 81 // * val = (first eigenvalue) --smallest --not important
shiyaozhanag 1:28c107f8cf24 82 // * .
shiyaozhanag 1:28c107f8cf24 83 // * .
shiyaozhanag 1:28c107f8cf24 84 // * .
shiyaozhanag 1:28c107f8cf24 85 // * (last eigenvalue) --largest -- important
shiyaozhanag 1:28c107f8cf24 86 // *
shiyaozhanag 1:28c107f8cf24 87 // * vec = (first eigenvector, ... , last eigenvector)
shiyaozhanag 1:28c107f8cf24 88 // * not important important
shiyaozhanag 1:28c107f8cf24 89 // */
shiyaozhanag 1:28c107f8cf24 90 //void ComputEig(MatrixXd &C, MatrixXd &vec, MatrixXd &val)
shiyaozhanag 1:28c107f8cf24 91 //{
shiyaozhanag 1:28c107f8cf24 92 // //SelfAdjointEigenSolver will sort the values automatically
shiyaozhanag 1:28c107f8cf24 93 // SelfAdjointEigenSolver<MatrixXd> eig(C);
shiyaozhanag 1:28c107f8cf24 94 // vec = eig.eigenvectors();
shiyaozhanag 1:28c107f8cf24 95 // val = eig.eigenvalues();
shiyaozhanag 1:28c107f8cf24 96 //}
shiyaozhanag 1:28c107f8cf24 97 //
shiyaozhanag 1:28c107f8cf24 98 ///* Compute the dimension need to include enough information of raw data.
shiyaozhanag 1:28c107f8cf24 99 // * form large index to small index, since the val is sorted from small to large.
shiyaozhanag 1:28c107f8cf24 100 // * in some cases, just decide the number of dimension, instead of compute it.
shiyaozhanag 1:28c107f8cf24 101 // */
shiyaozhanag 1:28c107f8cf24 102 //int ComputDim(MatrixXd &val)
shiyaozhanag 1:28c107f8cf24 103 //{
shiyaozhanag 1:28c107f8cf24 104 // int dim;
shiyaozhanag 1:28c107f8cf24 105 // double sum = 0;
shiyaozhanag 1:28c107f8cf24 106 // for (int i = val.rows() - 1; i >= 0;--i)
shiyaozhanag 1:28c107f8cf24 107 // {
shiyaozhanag 1:28c107f8cf24 108 // sum += val(i, 0);
shiyaozhanag 1:28c107f8cf24 109 // dim = i;
shiyaozhanag 1:28c107f8cf24 110 // if (sum / val.sum()>=0.8)//80% of the information
shiyaozhanag 1:28c107f8cf24 111 // break;
shiyaozhanag 1:28c107f8cf24 112 // }
shiyaozhanag 1:28c107f8cf24 113 // return val.rows() - dim;
shiyaozhanag 1:28c107f8cf24 114 //}
shiyaozhanag 1:28c107f8cf24 115 ***/
shiyaozhanag 1:28c107f8cf24 116
nikoletakuneva 0:dbcc46819a5e 117 static DigitalOut led1(LED1, 1);
nikoletakuneva 0:dbcc46819a5e 118
nikoletakuneva 0:dbcc46819a5e 119 const static char DEVICE_NAME[] = "STEP COUNTER";
nikoletakuneva 0:dbcc46819a5e 120
nikoletakuneva 0:dbcc46819a5e 121 const static uint16_t STEP_COUNTER_SERVICE_UUID = 0xA000;
nikoletakuneva 0:dbcc46819a5e 122 const static uint16_t STEP_COUNTER_CHARACTERISTIC_UUID = 0xA001;
nikoletakuneva 0:dbcc46819a5e 123
nikoletakuneva 0:dbcc46819a5e 124 int step_count = 0;
nikoletakuneva 0:dbcc46819a5e 125 int id = 0;
nikoletakuneva 0:dbcc46819a5e 126 ReadWriteGattCharacteristic<int> step_count_state(STEP_COUNTER_CHARACTERISTIC_UUID, &step_count, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
nikoletakuneva 0:dbcc46819a5e 127
nikoletakuneva 0:dbcc46819a5e 128 static events::EventQueue event_queue(/* event count */ 16 * EVENTS_EVENT_SIZE);
nikoletakuneva 0:dbcc46819a5e 129
nikoletakuneva 0:dbcc46819a5e 130 class StepCounter : ble::Gap::EventHandler {
nikoletakuneva 0:dbcc46819a5e 131 public:
nikoletakuneva 0:dbcc46819a5e 132 StepCounter(BLE &ble, events::EventQueue &event_queue) :
nikoletakuneva 0:dbcc46819a5e 133 _ble(ble),
nikoletakuneva 0:dbcc46819a5e 134 _event_queue(event_queue),
nikoletakuneva 0:dbcc46819a5e 135 _step_counter_uuid(STEP_COUNTER_SERVICE_UUID),
nikoletakuneva 0:dbcc46819a5e 136 _adv_data_builder(_adv_buffer) { }
nikoletakuneva 0:dbcc46819a5e 137
nikoletakuneva 0:dbcc46819a5e 138 void start() {
nikoletakuneva 0:dbcc46819a5e 139 _ble.gap().setEventHandler(this);
nikoletakuneva 0:dbcc46819a5e 140
nikoletakuneva 0:dbcc46819a5e 141 _ble.init(this, &StepCounter::on_init_complete);
nikoletakuneva 0:dbcc46819a5e 142 _event_queue.call_every(500, this, &StepCounter::blink);
shiyaozhanag 1:28c107f8cf24 143
shiyaozhanag 1:28c107f8cf24 144 // TODO Replace this to sync with data reading in order to update step
shiyaozhanag 1:28c107f8cf24 145 // counts by exact window.
nikoletakuneva 0:dbcc46819a5e 146 _event_queue.call_every(1000, this, &StepCounter::update_step_count);
nikoletakuneva 0:dbcc46819a5e 147
nikoletakuneva 0:dbcc46819a5e 148 _event_queue.dispatch_forever();
nikoletakuneva 0:dbcc46819a5e 149 }
nikoletakuneva 0:dbcc46819a5e 150
nikoletakuneva 0:dbcc46819a5e 151 private:
nikoletakuneva 0:dbcc46819a5e 152 /** Callback triggered when the ble initialization process has finished */
nikoletakuneva 0:dbcc46819a5e 153 void on_init_complete(BLE::InitializationCompleteCallbackContext *params) {
nikoletakuneva 0:dbcc46819a5e 154 if (params->error != BLE_ERROR_NONE) {
nikoletakuneva 0:dbcc46819a5e 155 print_error(params->error, "Ble initialization failed.");
nikoletakuneva 0:dbcc46819a5e 156 return;
nikoletakuneva 0:dbcc46819a5e 157 }
nikoletakuneva 0:dbcc46819a5e 158
nikoletakuneva 0:dbcc46819a5e 159 _ble.gattServer().onDataWritten(this, &StepCounter::on_data_written);
nikoletakuneva 0:dbcc46819a5e 160
nikoletakuneva 0:dbcc46819a5e 161 print_mac_address();
nikoletakuneva 0:dbcc46819a5e 162
nikoletakuneva 0:dbcc46819a5e 163 start_advertising();
nikoletakuneva 0:dbcc46819a5e 164 }
nikoletakuneva 0:dbcc46819a5e 165
nikoletakuneva 0:dbcc46819a5e 166 void start_advertising() {
nikoletakuneva 0:dbcc46819a5e 167 /* Create advertising parameters and payload */
nikoletakuneva 0:dbcc46819a5e 168
nikoletakuneva 0:dbcc46819a5e 169 ble::AdvertisingParameters adv_parameters(
nikoletakuneva 0:dbcc46819a5e 170 ble::advertising_type_t::CONNECTABLE_UNDIRECTED,
nikoletakuneva 0:dbcc46819a5e 171 ble::adv_interval_t(ble::millisecond_t(1000))
nikoletakuneva 0:dbcc46819a5e 172 );
nikoletakuneva 0:dbcc46819a5e 173
nikoletakuneva 0:dbcc46819a5e 174 _adv_data_builder.setFlags();
nikoletakuneva 0:dbcc46819a5e 175 _adv_data_builder.setLocalServiceList(mbed::make_Span(&_step_counter_uuid, 1));
nikoletakuneva 0:dbcc46819a5e 176 _adv_data_builder.setName(DEVICE_NAME);
nikoletakuneva 0:dbcc46819a5e 177
nikoletakuneva 0:dbcc46819a5e 178 /* Setup advertising */
nikoletakuneva 0:dbcc46819a5e 179
nikoletakuneva 0:dbcc46819a5e 180 ble_error_t error = _ble.gap().setAdvertisingParameters(
nikoletakuneva 0:dbcc46819a5e 181 ble::LEGACY_ADVERTISING_HANDLE,
nikoletakuneva 0:dbcc46819a5e 182 adv_parameters
nikoletakuneva 0:dbcc46819a5e 183 );
nikoletakuneva 0:dbcc46819a5e 184
nikoletakuneva 0:dbcc46819a5e 185 if (error) {
nikoletakuneva 0:dbcc46819a5e 186 print_error(error, "_ble.gap().setAdvertisingParameters() failed");
nikoletakuneva 0:dbcc46819a5e 187 return;
nikoletakuneva 0:dbcc46819a5e 188 }
nikoletakuneva 0:dbcc46819a5e 189
nikoletakuneva 0:dbcc46819a5e 190 error = _ble.gap().setAdvertisingPayload(
nikoletakuneva 0:dbcc46819a5e 191 ble::LEGACY_ADVERTISING_HANDLE,
nikoletakuneva 0:dbcc46819a5e 192 _adv_data_builder.getAdvertisingData()
nikoletakuneva 0:dbcc46819a5e 193 );
nikoletakuneva 0:dbcc46819a5e 194
nikoletakuneva 0:dbcc46819a5e 195 if (error) {
nikoletakuneva 0:dbcc46819a5e 196 print_error(error, "_ble.gap().setAdvertisingPayload() failed");
nikoletakuneva 0:dbcc46819a5e 197 return;
nikoletakuneva 0:dbcc46819a5e 198 }
nikoletakuneva 0:dbcc46819a5e 199
nikoletakuneva 0:dbcc46819a5e 200 /* Start advertising */
nikoletakuneva 0:dbcc46819a5e 201
nikoletakuneva 0:dbcc46819a5e 202 error = _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
nikoletakuneva 0:dbcc46819a5e 203
nikoletakuneva 0:dbcc46819a5e 204 if (error) {
nikoletakuneva 0:dbcc46819a5e 205 print_error(error, "_ble.gap().startAdvertising() failed");
nikoletakuneva 0:dbcc46819a5e 206 return;
nikoletakuneva 0:dbcc46819a5e 207 }
nikoletakuneva 0:dbcc46819a5e 208 }
nikoletakuneva 0:dbcc46819a5e 209
nikoletakuneva 0:dbcc46819a5e 210 void on_data_written(const GattWriteCallbackParams *params) {
nikoletakuneva 0:dbcc46819a5e 211 if ((params->handle == step_count_state.getValueHandle()) && (params->len == 1)) {
nikoletakuneva 0:dbcc46819a5e 212 step_count = *(params->data);
nikoletakuneva 0:dbcc46819a5e 213 }
nikoletakuneva 0:dbcc46819a5e 214 step_count = 0;
nikoletakuneva 0:dbcc46819a5e 215 }
nikoletakuneva 0:dbcc46819a5e 216
nikoletakuneva 0:dbcc46819a5e 217 void update_step_count() {
nikoletakuneva 0:dbcc46819a5e 218 if (_ble.gap().getState().connected) {
shiyaozhanag 1:28c107f8cf24 219 // TODO Remove step_count increament which would be implemented at
shiyaozhanag 1:28c107f8cf24 220 // main() after peak detection.
nikoletakuneva 0:dbcc46819a5e 221 step_count++;
nikoletakuneva 0:dbcc46819a5e 222
nikoletakuneva 0:dbcc46819a5e 223 _ble.gattServer().write(step_count_state.getValueHandle(), (uint8_t *)&step_count, sizeof(int));
nikoletakuneva 0:dbcc46819a5e 224 }
nikoletakuneva 0:dbcc46819a5e 225 }
nikoletakuneva 0:dbcc46819a5e 226
nikoletakuneva 0:dbcc46819a5e 227 void blink(void) {
nikoletakuneva 0:dbcc46819a5e 228 led1 = !led1;
nikoletakuneva 0:dbcc46819a5e 229 }
nikoletakuneva 0:dbcc46819a5e 230
nikoletakuneva 0:dbcc46819a5e 231 private:
nikoletakuneva 0:dbcc46819a5e 232 /* Event handler */
nikoletakuneva 0:dbcc46819a5e 233
nikoletakuneva 0:dbcc46819a5e 234 void onDisconnectionComplete(const ble::DisconnectionCompleteEvent&) {
nikoletakuneva 0:dbcc46819a5e 235 _ble.gap().startAdvertising(ble::LEGACY_ADVERTISING_HANDLE);
nikoletakuneva 0:dbcc46819a5e 236 }
nikoletakuneva 0:dbcc46819a5e 237
nikoletakuneva 0:dbcc46819a5e 238 private:
nikoletakuneva 0:dbcc46819a5e 239 BLE &_ble;
nikoletakuneva 0:dbcc46819a5e 240 events::EventQueue &_event_queue;
nikoletakuneva 0:dbcc46819a5e 241
nikoletakuneva 0:dbcc46819a5e 242 UUID _step_counter_uuid;
nikoletakuneva 0:dbcc46819a5e 243
nikoletakuneva 0:dbcc46819a5e 244 uint8_t _adv_buffer[ble::LEGACY_ADVERTISING_MAX_SIZE];
nikoletakuneva 0:dbcc46819a5e 245 ble::AdvertisingDataBuilder _adv_data_builder;
nikoletakuneva 0:dbcc46819a5e 246 };
nikoletakuneva 0:dbcc46819a5e 247
nikoletakuneva 0:dbcc46819a5e 248 /** Schedule processing of events from the BLE middleware in the event queue. */
nikoletakuneva 0:dbcc46819a5e 249 void schedule_ble_events(BLE::OnEventsToProcessCallbackContext *context) {
nikoletakuneva 0:dbcc46819a5e 250 event_queue.call(Callback<void()>(&context->ble, &BLE::processEvents));
nikoletakuneva 0:dbcc46819a5e 251 }
nikoletakuneva 0:dbcc46819a5e 252
nikoletakuneva 0:dbcc46819a5e 253 int main()
nikoletakuneva 0:dbcc46819a5e 254 {
nikoletakuneva 0:dbcc46819a5e 255 BLE &ble = BLE::Instance();
nikoletakuneva 0:dbcc46819a5e 256
nikoletakuneva 0:dbcc46819a5e 257 GattCharacteristic *charTable[] = {&step_count_state};
nikoletakuneva 0:dbcc46819a5e 258 GattService step_count_service(STEP_COUNTER_SERVICE_UUID, charTable, sizeof(charTable) / sizeof(GattCharacteristic *));
nikoletakuneva 0:dbcc46819a5e 259 ble.addService(step_count_service);
nikoletakuneva 0:dbcc46819a5e 260
nikoletakuneva 0:dbcc46819a5e 261 ble.onEventsToProcess(schedule_ble_events);
nikoletakuneva 0:dbcc46819a5e 262
nikoletakuneva 0:dbcc46819a5e 263 StepCounter demo(ble, event_queue);
nikoletakuneva 0:dbcc46819a5e 264 demo.start();
shiyaozhanag 1:28c107f8cf24 265
shiyaozhanag 1:28c107f8cf24 266 // read data
shiyaozhanag 1:28c107f8cf24 267 /***
shiyaozhanag 1:28c107f8cf24 268 PCA parameters init
shiyaozhanag 1:28c107f8cf24 269
shiyaozhanag 1:28c107f8cf24 270 //new mpu(data,clk,address),in constructor addr7bit<<1
shiyaozhanag 1:28c107f8cf24 271 mpu9250 *mpu = new mpu9250(p26,p27,addr7bit);
shiyaozhanag 1:28c107f8cf24 272 //scale of acc and gyro
shiyaozhanag 1:28c107f8cf24 273 mpu->initMPU9250(0x00,0x00);
shiyaozhanag 1:28c107f8cf24 274
shiyaozhanag 1:28c107f8cf24 275 float AccRead[3];
shiyaozhanag 1:28c107f8cf24 276 float GyroRead[3];
shiyaozhanag 1:28c107f8cf24 277 float TempRead[1];
shiyaozhanag 1:28c107f8cf24 278
shiyaozhanag 1:28c107f8cf24 279
shiyaozhanag 1:28c107f8cf24 280 MatrixXd acc_raw(3,0);
shiyaozhanag 1:28c107f8cf24 281 Vector3d acc_new;
shiyaozhanag 1:28c107f8cf24 282 MatrixXd C;
shiyaozhanag 1:28c107f8cf24 283 MatrixXd vec, val;
shiyaozhanag 1:28c107f8cf24 284 int dim = 1; //dimension of PCA
shiyaozhanag 1:28c107f8cf24 285 ***/
shiyaozhanag 1:28c107f8cf24 286 /***
shiyaozhanag 1:28c107f8cf24 287 Reading data and do PCA
shiyaozhanag 1:28c107f8cf24 288
shiyaozhanag 1:28c107f8cf24 289 while (true) {
shiyaozhanag 1:28c107f8cf24 290
shiyaozhanag 1:28c107f8cf24 291 //Blink LED and wait 1 seconds
shiyaozhanag 1:28c107f8cf24 292 // TODO Question actually: Is this to wait for collecting enough data
shiyaozhanag 1:28c107f8cf24 293 // for a window?
shiyaozhanag 1:28c107f8cf24 294 led1 = !led1;
shiyaozhanag 1:28c107f8cf24 295 thread_sleep_for(SLEEP_TIME);
shiyaozhanag 1:28c107f8cf24 296
shiyaozhanag 1:28c107f8cf24 297 //read and convert date
shiyaozhanag 1:28c107f8cf24 298 mpu->ReadConvertAll(AccRead,GyroRead,TempRead);
shiyaozhanag 1:28c107f8cf24 299 //printf("acc value is (%f,%f,%f).\n\r",AccRead[0],AccRead[1],AccRead[2]);
shiyaozhanag 1:28c107f8cf24 300 //printf("gyro value is (%f,%f,%f).\n\r",GyroRead[0],GyroRead[1],GyroRead[2]);
shiyaozhanag 1:28c107f8cf24 301 //printf("temp value is %f.\n\r",TempRead[0]);
shiyaozhanag 1:28c107f8cf24 302
shiyaozhanag 1:28c107f8cf24 303 //append new data to matrix acc_raw
shiyaozhanag 1:28c107f8cf24 304 //adding the columns
shiyaozhanag 1:28c107f8cf24 305 acc_new << AccRead[0],AccRead[1],AccRead[2];
shiyaozhanag 1:28c107f8cf24 306 acc_raw.conservativeResize(acc_raw.rows(), acc_raw.cols()+1);
shiyaozhanag 1:28c107f8cf24 307 acc_raw.col(acc_raw.cols()-1) = acc_new;
shiyaozhanag 1:28c107f8cf24 308 //cout << "acc_raw:" << acc_raw << endl;
shiyaozhanag 1:28c107f8cf24 309
shiyaozhanag 1:28c107f8cf24 310 // TODO Check if there are enough data for a single window
shiyaozhanag 1:28c107f8cf24 311 // if true -> run PCA and peak detection
shiyaozhanag 1:28c107f8cf24 312 // else -> sleep for another 1 second maybe?
shiyaozhanag 1:28c107f8cf24 313
shiyaozhanag 1:28c107f8cf24 314 //run PCA
shiyaozhanag 1:28c107f8cf24 315 MatrixXd X1=featurnormail(acc_raw);
shiyaozhanag 1:28c107f8cf24 316 ComComputeCov(X1, C);
shiyaozhanag 1:28c107f8cf24 317 ComputEig(C, vec, val);
shiyaozhanag 1:28c107f8cf24 318 //select dim num of eigenvector from right to left. right is important
shiyaozhanag 1:28c107f8cf24 319 //compute the result array
shiyaozhanag 1:28c107f8cf24 320 MatrixXd res = vec.rightCols(dim).transpose()*X1;
shiyaozhanag 1:28c107f8cf24 321
shiyaozhanag 1:28c107f8cf24 322 //show the result after PCA
shiyaozhanag 1:28c107f8cf24 323 //cout << "result" << res << endl;
shiyaozhanag 1:28c107f8cf24 324
shiyaozhanag 1:28c107f8cf24 325 // TODO add peak detection algorithm here
shiyaozhanag 1:28c107f8cf24 326
shiyaozhanag 1:28c107f8cf24 327 // TODO add step count. Also, think about lock of the variable - what
shiyaozhanag 1:28c107f8cf24 328 // if
shiyaozhanag 1:28c107f8cf24 329 }
shiyaozhanag 1:28c107f8cf24 330 ***/
shiyaozhanag 1:28c107f8cf24 331
nikoletakuneva 0:dbcc46819a5e 332
nikoletakuneva 0:dbcc46819a5e 333 return 0;
nikoletakuneva 0:dbcc46819a5e 334 }