Edson Alcala / Mbed OS StepTracking

Simple step tracking

Dependencies:   MPU9250 mbed-os

Fork of ST by alonso palomino

main.cpp@2:02175845b24c, 2017-10-15 (annotated)

Committer:
alonsopg
Date:
Sun Oct 15 18:24:03 2017 +0000
Revision:
2:02175845b24c
Parent:
1:3eec9883598a
Child:
3:3157e61f2bfd
before removing testing

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 1:3eec9883598a 13 bool testMPUConnection = true;
alonsopg 1:3eec9883598a 14 bool doSensorInitialization = false;
alonsopg 2:02175845b24c 15 bool printAccelerometer = false;
alonsopg 2:02175845b24c 16 bool printGyroscope = false;
alonsopg 2:02175845b24c 17
alonsopg 2:02175845b24c 18 DigitalOut led1(LED1);
alonsopg 2:02175845b24c 19 InterruptIn button(BUTTON1);
alonsopg 2:02175845b24c 20
alonsopg 2:02175845b24c 21 const static char DEVICE_NAME[] = "Terminator";
alonsopg 2:02175845b24c 22 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
alonsopg 2:02175845b24c 23
alonsopg 2:02175845b24c 24 enum {
alonsopg 2:02175845b24c 25 RELEASED = 0,
alonsopg 2:02175845b24c 26 PRESSED,
alonsopg 2:02175845b24c 27 IDLE
alonsopg 2:02175845b24c 28 };
alonsopg 2:02175845b24c 29
alonsopg 2:02175845b24c 30 static uint8_t buttonState = IDLE;
alonsopg 2:02175845b24c 31
alonsopg 2:02175845b24c 32 static ButtonService *buttonServicePtr;
alonsopg 2:02175845b24c 33
alonsopg 2:02175845b24c 34 void buttonPressedCallback(void)
alonsopg 2:02175845b24c 35 {
alonsopg 2:02175845b24c 36 /* Note that the buttonPressedCallback() executes in interrupt context, so it is safer to access
alonsopg 2:02175845b24c 37 * BLE device API from the main thread. */
alonsopg 2:02175845b24c 38 buttonState = PRESSED;
alonsopg 2:02175845b24c 39 }
alonsopg 2:02175845b24c 40
alonsopg 2:02175845b24c 41 void buttonReleasedCallback(void)
alonsopg 2:02175845b24c 42 {
alonsopg 2:02175845b24c 43 /* Note that the buttonReleasedCallback() executes in interrupt context, so it is safer to access
alonsopg 2:02175845b24c 44 * BLE device API from the main thread. */
alonsopg 2:02175845b24c 45 buttonState = RELEASED;
alonsopg 2:02175845b24c 46 }
alonsopg 2:02175845b24c 47
alonsopg 2:02175845b24c 48 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
alonsopg 2:02175845b24c 49 {
alonsopg 2:02175845b24c 50 BLE::Instance().gap().startAdvertising();
alonsopg 2:02175845b24c 51 }
alonsopg 2:02175845b24c 52
alonsopg 2:02175845b24c 53 void periodicFunctionCallback(void)
alonsopg 2:02175845b24c 54 {
alonsopg 2:02175845b24c 55 led1 = !led1; /* Do blinky on LED1 to indicate system aliveness. */
alonsopg 2:02175845b24c 56 }
alonsopg 2:02175845b24c 57
alonsopg 2:02175845b24c 58 /**
alonsopg 2:02175845b24c 59 * This function is called when the ble initialization process has failled
alonsopg 2:02175845b24c 60 */
alonsopg 2:02175845b24c 61 void onBleInitError(BLE &ble, ble_error_t error)
alonsopg 2:02175845b24c 62 {
alonsopg 2:02175845b24c 63 /* Initialization error handling should go here */
alonsopg 2:02175845b24c 64 }
alonsopg 0:30a995e45e2a 65
alonsopg 2:02175845b24c 66 /**
alonsopg 2:02175845b24c 67 * Callback triggered when the ble initialization process has finished
alonsopg 2:02175845b24c 68 */
alonsopg 2:02175845b24c 69 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
alonsopg 2:02175845b24c 70 {
alonsopg 2:02175845b24c 71 BLE& ble = params->ble;
alonsopg 2:02175845b24c 72 ble_error_t error = params->error;
alonsopg 2:02175845b24c 73
alonsopg 2:02175845b24c 74 if (error != BLE_ERROR_NONE) {
alonsopg 2:02175845b24c 75 /* In case of error, forward the error handling to onBleInitError */
alonsopg 2:02175845b24c 76 onBleInitError(ble, error);
alonsopg 2:02175845b24c 77 return;
alonsopg 2:02175845b24c 78 }
alonsopg 2:02175845b24c 79
alonsopg 2:02175845b24c 80 /* Ensure that it is the default instance of BLE */
alonsopg 2:02175845b24c 81 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
alonsopg 2:02175845b24c 82 return;
alonsopg 2:02175845b24c 83 }
alonsopg 2:02175845b24c 84
alonsopg 2:02175845b24c 85 ble.gap().onDisconnection(disconnectionCallback);
alonsopg 2:02175845b24c 86
alonsopg 2:02175845b24c 87 /* Setup primary service */
alonsopg 2:02175845b24c 88 buttonServicePtr = new ButtonService(ble, false /* initial value for button pressed */);
alonsopg 2:02175845b24c 89
alonsopg 2:02175845b24c 90 /* setup advertising */
alonsopg 2:02175845b24c 91 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
alonsopg 2:02175845b24c 92 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
alonsopg 2:02175845b24c 93 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
alonsopg 2:02175845b24c 94 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
alonsopg 2:02175845b24c 95 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
alonsopg 2:02175845b24c 96 ble.gap().startAdvertising();
alonsopg 2:02175845b24c 97
alonsopg 2:02175845b24c 98 }
alonsopg 2:02175845b24c 99
alonsopg 2:02175845b24c 100 int main ()
alonsopg 2:02175845b24c 101 {
alonsopg 2:02175845b24c 102 // Turn the led
alonsopg 2:02175845b24c 103 led1 = 1;
alonsopg 1:3eec9883598a 104 int16_t accelerometer[3] = {0,0,0};
alonsopg 2:02175845b24c 105 int16_t gyroscope[3] = {0,0,0};
alonsopg 2:02175845b24c 106
alonsopg 2:02175845b24c 107 //Attach a function to be called by the Ticker, specifiying the interval in seconds.
alonsopg 2:02175845b24c 108 Ticker ticker;
alonsopg 2:02175845b24c 109 ticker.attach(periodicFunctionCallback, 1);
alonsopg 2:02175845b24c 110 button.fall(buttonPressedCallback);
alonsopg 2:02175845b24c 111 button.rise(buttonReleasedCallback);
alonsopg 2:02175845b24c 112
alonsopg 2:02175845b24c 113 BLE &ble = BLE::Instance();
alonsopg 2:02175845b24c 114 ble.init(bleInitComplete);
alonsopg 2:02175845b24c 115
alonsopg 2:02175845b24c 116 /* SpinWait for initialization to complete. This is necessary because the
alonsopg 2:02175845b24c 117 * BLE object is used in the main loop below. */
alonsopg 2:02175845b24c 118 while (ble.hasInitialized() == false) {
alonsopg 2:02175845b24c 119 /* spin loop */
alonsopg 2:02175845b24c 120 }
alonsopg 2:02175845b24c 121
alonsopg 1:3eec9883598a 122 if (testMPUConnection) {
alonsopg 0:30a995e45e2a 123 uint8_t whoami = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
alonsopg 2:02175845b24c 124 pc.printf("I AM 0x%x\n\r", whoami);
alonsopg 0:30a995e45e2a 125 pc.printf("I SHOULD BE 0x71\n\r");
alonsopg 2:02175845b24c 126 wait(1);
alonsopg 2:02175845b24c 127 }
alonsopg 2:02175845b24c 128
alonsopg 1:3eec9883598a 129 if (doSensorInitialization) {
alonsopg 2:02175845b24c 130 // Initialise sensor board
alonsopg 1:3eec9883598a 131 pc.printf("Initializing sensor\n\r");
alonsopg 1:3eec9883598a 132 mpu.initMPU9250();
alonsopg 1:3eec9883598a 133 pc.printf("Initialization finished\n\r");
alonsopg 1:3eec9883598a 134 wait(1);
alonsopg 1:3eec9883598a 135 }
alonsopg 2:02175845b24c 136
alonsopg 0:30a995e45e2a 137 while(1) {
alonsopg 2:02175845b24c 138
alonsopg 2:02175845b24c 139 //pc.printf("Starting to stream data");
alonsopg 2:02175845b24c 140 if(printAccelerometer && printGyroscope) {
alonsopg 1:3eec9883598a 141 mpu.readAccelData(accelerometer);
alonsopg 1:3eec9883598a 142 float ax = accelerometer[0] * 2.0 / 32768.0;
alonsopg 1:3eec9883598a 143 float ay = accelerometer[1] * 2.0 / 32768.0;
alonsopg 1:3eec9883598a 144 float az = accelerometer[2] * 2.0 / 32768.0;
alonsopg 2:02175845b24c 145
alonsopg 2:02175845b24c 146 //pc.printf("Acelerometer information, AX, AY, AZ \n");
alonsopg 2:02175845b24c 147 //pc.printf("(%f, %f, %f)\n", ax,ay,az);
alonsopg 2:02175845b24c 148
alonsopg 1:3eec9883598a 149 float roll = float(atan2(ay, az) * 180/3.1416f);
alonsopg 1:3eec9883598a 150 float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f);
alonsopg 1:3eec9883598a 151 float yaw = atan(ax/-ay);
alonsopg 2:02175845b24c 152
alonsopg 2:02175845b24c 153 //pc.printf("Roll/Pitch/Yaw: (%f, %f, %f)\n", roll, pitch, yaw);
alonsopg 2:02175845b24c 154
alonsopg 1:3eec9883598a 155 mpu.readGyroData(gyroscope);
alonsopg 1:3eec9883598a 156 float gx = gyroscope[0] * 250.0 / 32768.0;
alonsopg 1:3eec9883598a 157 float gy = gyroscope[1] * 250.0 / 32768.0;
alonsopg 1:3eec9883598a 158 float gz = gyroscope[2] * 250.0 / 32768.0;
alonsopg 2:02175845b24c 159
alonsopg 2:02175845b24c 160
alonsopg 2:02175845b24c 161 pc.printf("(%f, %f, %f, %f, %f, %f)\n", roll, pitch, yaw, gx, gy, gz);
alonsopg 2:02175845b24c 162
alonsopg 2:02175845b24c 163
alonsopg 1:3eec9883598a 164 }
alonsopg 2:02175845b24c 165 if (buttonState != IDLE) {
alonsopg 2:02175845b24c 166 //buttonServicePtr->updateButtonState(buttonState);
alonsopg 2:02175845b24c 167 //char myword[] = { 'H', 'e', 'l', 'l', 'o', '\0' };
alonsopg 2:02175845b24c 168 uint8_t sensorValues[12] = {0};
alonsopg 2:02175845b24c 169 sensorValues[0] = 12;
alonsopg 2:02175845b24c 170 sensorValues[1] = 12;
alonsopg 2:02175845b24c 171 sensorValues[2] = 12;
alonsopg 2:02175845b24c 172 sensorValues[3] = 12;
alonsopg 2:02175845b24c 173 sensorValues[4] = 12;
alonsopg 2:02175845b24c 174 sensorValues[5] = 12;
alonsopg 2:02175845b24c 175 sensorValues[6] = 12;
alonsopg 2:02175845b24c 176 sensorValues[7] = 12;
alonsopg 2:02175845b24c 177 sensorValues[8] = 12;
alonsopg 2:02175845b24c 178 sensorValues[9] = 12;
alonsopg 2:02175845b24c 179 sensorValues[10] = 12;
alonsopg 2:02175845b24c 180 sensorValues[11] = 12;
alonsopg 2:02175845b24c 181 buttonServicePtr->updateButtonState(sensorValues);
alonsopg 2:02175845b24c 182 buttonState = IDLE;
alonsopg 2:02175845b24c 183 }
alonsopg 2:02175845b24c 184 ble.waitForEvent();
alonsopg 2:02175845b24c 185
alonsopg 2:02175845b24c 186 //wait(0.3);
alonsopg 2:02175845b24c 187 }
alonsopg 0:30a995e45e2a 188 }