Edson Alcala / Mbed OS StepTracking

Simple step tracking

Dependencies:   MPU9250 mbed-os

Fork of ST by alonso palomino

main.cpp@6:a400e1e47156, 2017-10-19 (annotated)

Committer:
EdsonAlcala
Date:
Thu Oct 19 21:59:43 2017 +0000
Revision:
6:a400e1e47156
Parent:
4:b741278722c1
Child:
9:e265a634306d
la buena

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alonsopg 0:30a995e45e2a 1 #include "mbed.h"
alonsopg 0:30a995e45e2a 2 #include "MPU9250.h"
alonsopg 2:02175845b24c 3 #include "ble/BLE.h"
alonsopg 2:02175845b24c 4 #include "ButtonService.h"
alonsopg 0:30a995e45e2a 5
alonsopg 0:30a995e45e2a 6 // Serial comms
alonsopg 0:30a995e45e2a 7 Serial pc(USBTX, USBRX);
alonsopg 0:30a995e45e2a 8
alonsopg 0:30a995e45e2a 9 // Sensor board library
alonsopg 0:30a995e45e2a 10 MPU9250 mpu = MPU9250(p26, p27);
alonsopg 0:30a995e45e2a 11
alonsopg 0:30a995e45e2a 12 // Configuration
alonsopg 3:3157e61f2bfd 13 bool doSensorInitialization = true;
alonsopg 3:3157e61f2bfd 14 bool printAccelerometer = true;
alonsopg 3:3157e61f2bfd 15 bool printGyroscope = true;
alonsopg 2:02175845b24c 16
alonsopg 2:02175845b24c 17 DigitalOut led1(LED1);
alonsopg 2:02175845b24c 18 InterruptIn button(BUTTON1);
alonsopg 2:02175845b24c 19
alonsopg 2:02175845b24c 20 const static char DEVICE_NAME[] = "Terminator";
alonsopg 2:02175845b24c 21 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
alonsopg 2:02175845b24c 22
alonsopg 2:02175845b24c 23 enum {
alonsopg 2:02175845b24c 24 RELEASED = 0,
alonsopg 2:02175845b24c 25 PRESSED,
alonsopg 2:02175845b24c 26 IDLE
alonsopg 2:02175845b24c 27 };
alonsopg 2:02175845b24c 28
alonsopg 2:02175845b24c 29 static ButtonService *buttonServicePtr;
alonsopg 2:02175845b24c 30
alonsopg 2:02175845b24c 31 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
alonsopg 2:02175845b24c 32 {
alonsopg 2:02175845b24c 33 BLE::Instance().gap().startAdvertising();
alonsopg 2:02175845b24c 34 }
alonsopg 2:02175845b24c 35
alonsopg 2:02175845b24c 36 void periodicFunctionCallback(void)
alonsopg 2:02175845b24c 37 {
alonsopg 2:02175845b24c 38 led1 = !led1; /* Do blinky on LED1 to indicate system aliveness. */
alonsopg 2:02175845b24c 39 }
alonsopg 2:02175845b24c 40
alonsopg 2:02175845b24c 41 /**
alonsopg 2:02175845b24c 42 * This function is called when the ble initialization process has failled
alonsopg 2:02175845b24c 43 */
alonsopg 2:02175845b24c 44 void onBleInitError(BLE &ble, ble_error_t error)
alonsopg 2:02175845b24c 45 {
alonsopg 2:02175845b24c 46 /* Initialization error handling should go here */
alonsopg 2:02175845b24c 47 }
alonsopg 0:30a995e45e2a 48
alonsopg 2:02175845b24c 49 /**
alonsopg 2:02175845b24c 50 * Callback triggered when the ble initialization process has finished
alonsopg 2:02175845b24c 51 */
alonsopg 2:02175845b24c 52 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
alonsopg 2:02175845b24c 53 {
alonsopg 2:02175845b24c 54 BLE& ble = params->ble;
alonsopg 2:02175845b24c 55 ble_error_t error = params->error;
alonsopg 2:02175845b24c 56
alonsopg 2:02175845b24c 57 if (error != BLE_ERROR_NONE) {
alonsopg 2:02175845b24c 58 /* In case of error, forward the error handling to onBleInitError */
alonsopg 2:02175845b24c 59 onBleInitError(ble, error);
alonsopg 2:02175845b24c 60 return;
alonsopg 2:02175845b24c 61 }
alonsopg 2:02175845b24c 62
alonsopg 2:02175845b24c 63 /* Ensure that it is the default instance of BLE */
alonsopg 2:02175845b24c 64 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
alonsopg 2:02175845b24c 65 return;
alonsopg 2:02175845b24c 66 }
alonsopg 2:02175845b24c 67
alonsopg 2:02175845b24c 68 ble.gap().onDisconnection(disconnectionCallback);
alonsopg 2:02175845b24c 69
alonsopg 2:02175845b24c 70 /* Setup primary service */
alonsopg 2:02175845b24c 71 buttonServicePtr = new ButtonService(ble, false /* initial value for button pressed */);
alonsopg 2:02175845b24c 72
alonsopg 2:02175845b24c 73 /* setup advertising */
alonsopg 2:02175845b24c 74 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
alonsopg 2:02175845b24c 75 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
alonsopg 2:02175845b24c 76 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
alonsopg 2:02175845b24c 77 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
alonsopg 2:02175845b24c 78 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
alonsopg 2:02175845b24c 79 ble.gap().startAdvertising();
alonsopg 2:02175845b24c 80
alonsopg 2:02175845b24c 81 }
alonsopg 2:02175845b24c 82
alonsopg 2:02175845b24c 83 int main ()
alonsopg 2:02175845b24c 84 {
alonsopg 2:02175845b24c 85 // Turn the led
alonsopg 2:02175845b24c 86 led1 = 1;
alonsopg 1:3eec9883598a 87 int16_t accelerometer[3] = {0,0,0};
alonsopg 2:02175845b24c 88 int16_t gyroscope[3] = {0,0,0};
alonsopg 2:02175845b24c 89
alonsopg 2:02175845b24c 90 //Attach a function to be called by the Ticker, specifiying the interval in seconds.
alonsopg 2:02175845b24c 91 Ticker ticker;
alonsopg 3:3157e61f2bfd 92 ticker.attach(periodicFunctionCallback, 1);
alonsopg 2:02175845b24c 93
alonsopg 2:02175845b24c 94 BLE &ble = BLE::Instance();
alonsopg 2:02175845b24c 95 ble.init(bleInitComplete);
alonsopg 2:02175845b24c 96
alonsopg 2:02175845b24c 97 /* SpinWait for initialization to complete. This is necessary because the
alonsopg 2:02175845b24c 98 * BLE object is used in the main loop below. */
alonsopg 2:02175845b24c 99 while (ble.hasInitialized() == false) {
alonsopg 2:02175845b24c 100 /* spin loop */
alonsopg 2:02175845b24c 101 }
alonsopg 2:02175845b24c 102
alonsopg 1:3eec9883598a 103 if (doSensorInitialization) {
alonsopg 2:02175845b24c 104 // Initialise sensor board
alonsopg 1:3eec9883598a 105 pc.printf("Initializing sensor\n\r");
alonsopg 1:3eec9883598a 106 mpu.initMPU9250();
alonsopg 1:3eec9883598a 107 pc.printf("Initialization finished\n\r");
alonsopg 1:3eec9883598a 108 wait(1);
alonsopg 1:3eec9883598a 109 }
alonsopg 2:02175845b24c 110
alonsopg 0:30a995e45e2a 111 while(1) {
alonsopg 2:02175845b24c 112
alonsopg 2:02175845b24c 113 if(printAccelerometer && printGyroscope) {
alonsopg 1:3eec9883598a 114 mpu.readAccelData(accelerometer);
alonsopg 1:3eec9883598a 115 float ax = accelerometer[0] * 2.0 / 32768.0;
alonsopg 1:3eec9883598a 116 float ay = accelerometer[1] * 2.0 / 32768.0;
alonsopg 1:3eec9883598a 117 float az = accelerometer[2] * 2.0 / 32768.0;
alonsopg 2:02175845b24c 118
alonsopg 1:3eec9883598a 119 float roll = float(atan2(ay, az) * 180/3.1416f);
alonsopg 1:3eec9883598a 120 float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f);
alonsopg 1:3eec9883598a 121 float yaw = atan(ax/-ay);
alonsopg 2:02175845b24c 122
alonsopg 1:3eec9883598a 123 mpu.readGyroData(gyroscope);
alonsopg 1:3eec9883598a 124 float gx = gyroscope[0] * 250.0 / 32768.0;
alonsopg 1:3eec9883598a 125 float gy = gyroscope[1] * 250.0 / 32768.0;
alonsopg 1:3eec9883598a 126 float gz = gyroscope[2] * 250.0 / 32768.0;
EdsonAlcala 4:b741278722c1 127
EdsonAlcala 6:a400e1e47156 128 uint8_t *gzValue;
EdsonAlcala 6:a400e1e47156 129 gzValue = reinterpret_cast<uint8_t*>(&gz);
EdsonAlcala 6:a400e1e47156 130 pc.printf("SIZE TEST %d \n", sizeof(gzValue));
EdsonAlcala 4:b741278722c1 131 pc.printf("%f, %f, %f, %f, %f, %f \n", roll, pitch, yaw, gx, gy, gz);
alonsopg 3:3157e61f2bfd 132 buttonServicePtr->updateButtonState(roll, pitch, yaw, gx, gy, gz);
EdsonAlcala 4:b741278722c1 133
alonsopg 1:3eec9883598a 134 }
alonsopg 2:02175845b24c 135 ble.waitForEvent();
alonsopg 2:02175845b24c 136
alonsopg 3:3157e61f2bfd 137 wait(0.3);
alonsopg 2:02175845b24c 138 }
alonsopg 0:30a995e45e2a 139 }