Edson Alcala
/
StepTracking
Simple step tracking
Fork of ST by
main.cpp
- Committer:
- EdsonAlcala
- Date:
- 2017-10-22
- Revision:
- 11:03a4a6e9f27c
- Parent:
- 9:e265a634306d
File content as of revision 11:03a4a6e9f27c:
#include "mbed.h" #include "MPU9250.h" #include "ble/BLE.h" #include "ButtonService.h" // Serial comms Serial pc(USBTX, USBRX); // Sensor board library MPU9250 mpu = MPU9250(p26, p27); // Configuration bool doSensorInitialization = true; bool printAccelerometer = true; bool printGyroscope = true; DigitalOut led1(LED1); InterruptIn button(BUTTON1); const static char DEVICE_NAME[] = "Terminator"; static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID}; enum { RELEASED = 0, PRESSED, IDLE }; static ButtonService *buttonServicePtr; void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) { BLE::Instance().gap().startAdvertising(); } void periodicFunctionCallback(void) { led1 = !led1; /* Do blinky on LED1 to indicate system aliveness. */ } /** * This function is called when the ble initialization process has failled */ void onBleInitError(BLE &ble, ble_error_t error) { /* Initialization error handling should go here */ } /** * Callback triggered when the ble initialization process has finished */ void bleInitComplete(BLE::InitializationCompleteCallbackContext *params) { BLE& ble = params->ble; ble_error_t error = params->error; if (error != BLE_ERROR_NONE) { /* In case of error, forward the error handling to onBleInitError */ onBleInitError(ble, error); return; } /* Ensure that it is the default instance of BLE */ if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) { return; } ble.gap().onDisconnection(disconnectionCallback); /* Setup primary service */ buttonServicePtr = new ButtonService(ble, false /* initial value for button pressed */); /* setup advertising */ ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); ble.gap().setAdvertisingInterval(1000); /* 1000ms. */ ble.gap().startAdvertising(); } int main () { // Turn the led led1 = 1; int16_t accelerometer[3] = {0,0,0}; int16_t gyroscope[3] = {0,0,0}; //Attach a function to be called by the Ticker, specifiying the interval in seconds. Ticker ticker; ticker.attach(periodicFunctionCallback, 1); BLE &ble = BLE::Instance(); ble.init(bleInitComplete); /* SpinWait for initialization to complete. This is necessary because the * BLE object is used in the main loop below. */ while (ble.hasInitialized() == false) { /* spin loop */ } if (doSensorInitialization) { // Initialise sensor board pc.printf("Initializing sensor\n\r"); mpu.initMPU9250(); pc.printf("Initialization finished\n\r"); wait(1); } while(1) { if(printAccelerometer && printGyroscope) { mpu.readAccelData(accelerometer); float ax = accelerometer[0] * 2.0 / 32768.0; float ay = accelerometer[1] * 2.0 / 32768.0; float az = accelerometer[2] * 2.0 / 32768.0; float roll = float(atan2(ay, az) * 180/3.1416f); float pitch = float(atan2(-ax, sqrt(ay*ay + az*az)) * 180/3.1416f); float yaw = atan(ax/-ay); mpu.readGyroData(gyroscope); float gx = gyroscope[0] * 250.0 / 32768.0; float gy = gyroscope[1] * 250.0 / 32768.0; float gz = gyroscope[2] * 250.0 / 32768.0; pc.printf("%f, %f, %f, %f, %f, %f \n", roll, pitch, yaw, gx, gy, gz); buttonServicePtr->updateButtonState(roll, pitch, yaw, gyroscope[0], gyroscope[1], gyroscope[2]); } ble.waitForEvent(); wait(0.3); } }