Suad Suljic / Mbed OS a

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   SDFileSystem circular_buffer MPU6050 SoftSerial

source/main.cpp@7:bc915651d90e, 2018-11-23 (annotated)

Committer:
suads
Date:
Fri Nov 23 13:44:27 2018 +0000
Revision:
7:bc915651d90e
Parent:
5:ea629a3fd6c1 
seismo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
suads 5:ea629a3fd6c1 1 //#include <events/mbed_events.h>
OsmanKameric 0:a4de55cab4e2 2 #include <mbed.h>
OsmanKameric 0:a4de55cab4e2 3 #include "MPU6050.h"
OsmanKameric 4:147bbe6f9626 4 #include <string>
OsmanKameric 1:2cc1c9a45be7 5 #include <stdio.h>
suads 7:bc915651d90e 6 #include <events/mbed_events.h>
suads 7:bc915651d90e 7 #include "ble/BLE.h"
suads 7:bc915651d90e 8 #include "ble/Gap.h"
suads 7:bc915651d90e 9 #include "MotorService.h"
OsmanKameric 4:147bbe6f9626 10
suads 7:bc915651d90e 11 #define N_128_BIT_SERVICE 2
suads 7:bc915651d90e 12
suads 7:bc915651d90e 13 static uint8_t UUID_128_LIST[N_128_BIT_SERVICE*UUID::LENGTH_OF_LONG_UUID];
suads 7:bc915651d90e 14 static uint8_t uuid128ListPos = 0;
suads 7:bc915651d90e 15 static uint8_t uuid128ListAddedServices = 0;
suads 7:bc915651d90e 16
suads 7:bc915651d90e 17 static MotorService* motorServicePtr;
suads 7:bc915651d90e 18
suads 7:bc915651d90e 19 DigitalOut indicatorLED(p25, 0);
suads 7:bc915651d90e 20 MPU6050 mpu6050;
suads 7:bc915651d90e 21 Serial pc(USBTX,USBRX);
suads 7:bc915651d90e 22
suads 7:bc915651d90e 23
suads 7:bc915651d90e 24 const static char DEVICE_NAME[] = "Spike";
suads 7:bc915651d90e 25 // static const uint16_t uuid16_list[] = {GattService::UUID_BATTERY_SERVICE};
suads 7:bc915651d90e 26 static EventQueue eventQueue(/* event count */ 16 * EVENTS_EVENT_SIZE);
suads 7:bc915651d90e 27
suads 7:bc915651d90e 28
suads 7:bc915651d90e 29 void add128bitServiceToList(const uint8_t * service){
suads 7:bc915651d90e 30 uuid128ListAddedServices++;
suads 7:bc915651d90e 31 for(int i=UUID::LENGTH_OF_LONG_UUID-1; i>=0; i--){
suads 7:bc915651d90e 32 UUID_128_LIST[uuid128ListPos] = service[i];
suads 7:bc915651d90e 33 uuid128ListPos++;
suads 7:bc915651d90e 34 }
suads 7:bc915651d90e 35 }
suads 7:bc915651d90e 36
suads 7:bc915651d90e 37 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params){
suads 7:bc915651d90e 38 BLE::Instance().gap().startAdvertising();
suads 7:bc915651d90e 39 }
suads 7:bc915651d90e 40
suads 7:bc915651d90e 41 void connectionStatusIndicator(void) {
suads 7:bc915651d90e 42 BLE &ble = BLE::Instance();
suads 7:bc915651d90e 43 if (ble.gap().getState().connected) {
suads 7:bc915651d90e 44 indicatorLED = 1;
suads 7:bc915651d90e 45 //eventQueue.call(updateSensorValue);
suads 7:bc915651d90e 46 } else {
suads 7:bc915651d90e 47 indicatorLED = 0;
suads 7:bc915651d90e 48 }
suads 7:bc915651d90e 49 }
suads 7:bc915651d90e 50
suads 7:bc915651d90e 51 void onBleInitError(BLE &ble, ble_error_t error) {
suads 7:bc915651d90e 52 /* Initialization error handling should go here */
suads 7:bc915651d90e 53 }
suads 7:bc915651d90e 54
suads 7:bc915651d90e 55 void onDataWrittenCallback(const GattWriteCallbackParams *params) {
suads 7:bc915651d90e 56
suads 7:bc915651d90e 57 }
suads 7:bc915651d90e 58
suads 7:bc915651d90e 59 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
suads 7:bc915651d90e 60 {
suads 7:bc915651d90e 61 BLE& ble = params->ble;
suads 7:bc915651d90e 62 ble_error_t error = params->error;
suads 7:bc915651d90e 63
suads 7:bc915651d90e 64 if (error != BLE_ERROR_NONE) {
suads 7:bc915651d90e 65 /* In case of error, forward the error handling to onBleInitError */
suads 7:bc915651d90e 66 onBleInitError(ble, error);
suads 7:bc915651d90e 67 return;
suads 7:bc915651d90e 68 }
suads 7:bc915651d90e 69 /* Ensure that it is the default instance of BLE */
suads 7:bc915651d90e 70 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
suads 7:bc915651d90e 71 return;
suads 7:bc915651d90e 72 }
OsmanKameric 4:147bbe6f9626 73
OsmanKameric 4:147bbe6f9626 74
suads 7:bc915651d90e 75 ble.gap().onDisconnection(disconnectionCallback);
suads 7:bc915651d90e 76 ble.gattServer().onDataWritten(onDataWrittenCallback);
suads 7:bc915651d90e 77
suads 7:bc915651d90e 78 /* Setup GATT services */
suads 7:bc915651d90e 79 motorServicePtr = new MotorService(ble, 0, 0, 0);
suads 7:bc915651d90e 80 add128bitServiceToList(MOTOR_SERVICE_LONG_UUID);
suads 7:bc915651d90e 81 /* Setup advertising */
suads 7:bc915651d90e 82 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
suads 7:bc915651d90e 83 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *) DEVICE_NAME, sizeof(DEVICE_NAME));
suads 7:bc915651d90e 84 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
suads 7:bc915651d90e 85 ble.gap().setAdvertisingInterval(1000); /* 1000ms */
suads 7:bc915651d90e 86
suads 7:bc915651d90e 87 //ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *) uuid16_list, sizeof(uuid16_list));
suads 7:bc915651d90e 88 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
suads 7:bc915651d90e 89 (const uint8_t *)UUID_128_LIST, uuid128ListAddedServices*UUID::LENGTH_OF_LONG_UUID);
suads 7:bc915651d90e 90
suads 7:bc915651d90e 91 ble.gap().startAdvertising();
suads 7:bc915651d90e 92
suads 7:bc915651d90e 93 //printMacAddress();
suads 7:bc915651d90e 94 }
suads 7:bc915651d90e 95
suads 7:bc915651d90e 96 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context) {
suads 7:bc915651d90e 97 BLE &ble = BLE::Instance();
suads 7:bc915651d90e 98 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
suads 7:bc915651d90e 99 }
suads 7:bc915651d90e 100
suads 7:bc915651d90e 101 int i = 0;
suads 7:bc915651d90e 102
suads 7:bc915651d90e 103 void updateAccelValues(){
suads 7:bc915651d90e 104
suads 7:bc915651d90e 105 // mpu6050.readAccelData(accelData);
suads 7:bc915651d90e 106 // mpu6050.getAres();
suads 7:bc915651d90e 107 // float ax = accelData[0]*aRes - accelBias[0];
suads 7:bc915651d90e 108 // float ay = accelData[1]*aRes - accelBias[1];
suads 7:bc915651d90e 109 // float az = accelData[2]*aRes - accelBias[2];
suads 7:bc915651d90e 110 // float accelValues[3] = {ax,ay,az};
suads 7:bc915651d90e 111 mpu6050.readGyroData(gyroData);
suads 7:bc915651d90e 112 mpu6050.getGres();
suads 7:bc915651d90e 113 float gx = gyroData[0]*gRes - gyroBias[0];
suads 7:bc915651d90e 114 float gy = gyroData[1]*gRes - gyroBias[1];
suads 7:bc915651d90e 115 float gz = gyroData[2]*gRes - gyroBias[2];
suads 7:bc915651d90e 116 float gyroValues[3] = {gx,gy,gz};
suads 7:bc915651d90e 117
suads 7:bc915651d90e 118 pc.printf("%f, ",gx);
suads 7:bc915651d90e 119 pc.printf("%f, ",gy);
suads 7:bc915651d90e 120 pc.printf("%f\r\n",gz);
suads 7:bc915651d90e 121
suads 7:bc915651d90e 122 BLE &ble = BLE::Instance();
suads 7:bc915651d90e 123 if (ble.gap().getState().connected) {
suads 7:bc915651d90e 124 // update char vals
suads 7:bc915651d90e 125 ble.gattServer().write(
suads 7:bc915651d90e 126 motorServicePtr->getValueHandleMotor1(),
suads 7:bc915651d90e 127 (const uint8_t*)&gx,
suads 7:bc915651d90e 128 4
suads 7:bc915651d90e 129 );
suads 7:bc915651d90e 130 ble.gattServer().write(
suads 7:bc915651d90e 131 motorServicePtr->getValueHandleMotor2(),
suads 7:bc915651d90e 132 (const uint8_t*)&gy,
suads 7:bc915651d90e 133 4
suads 7:bc915651d90e 134 );
suads 7:bc915651d90e 135 ble.gattServer().write(
suads 7:bc915651d90e 136 motorServicePtr->getValueHandleMotor3(),
suads 7:bc915651d90e 137 (const uint8_t*)&gz,
suads 7:bc915651d90e 138 4
suads 7:bc915651d90e 139 );
suads 7:bc915651d90e 140 }
suads 7:bc915651d90e 141 }
suads 7:bc915651d90e 142
suads 7:bc915651d90e 143 void blink(){
suads 7:bc915651d90e 144 indicatorLED = 1;
suads 7:bc915651d90e 145 wait(1);
suads 7:bc915651d90e 146 indicatorLED = 0;
suads 7:bc915651d90e 147 }
OsmanKameric 4:147bbe6f9626 148
suads 5:ea629a3fd6c1 149 int main()
OsmanKameric 4:147bbe6f9626 150 {
suads 5:ea629a3fd6c1 151 pc.baud(115200);
suads 5:ea629a3fd6c1 152 pc.printf("START_1\n");
suads 7:bc915651d90e 153
suads 5:ea629a3fd6c1 154 mpu6050.calibrate(accelBias, gyroBias);
suads 5:ea629a3fd6c1 155 mpu6050.initialize();
suads 7:bc915651d90e 156 blink();
suads 5:ea629a3fd6c1 157 pc.printf("START_2\n");
suads 5:ea629a3fd6c1 158
suads 7:bc915651d90e 159
suads 7:bc915651d90e 160 eventQueue.call_every(2000, connectionStatusIndicator);
suads 7:bc915651d90e 161 eventQueue.call_every(200, updateAccelValues);
suads 7:bc915651d90e 162
suads 7:bc915651d90e 163 BLE &ble = BLE::Instance();
suads 7:bc915651d90e 164 ble.onEventsToProcess(scheduleBleEventsProcessing);
suads 7:bc915651d90e 165 ble.init(bleInitComplete);
suads 7:bc915651d90e 166
suads 7:bc915651d90e 167 blink();
suads 7:bc915651d90e 168 eventQueue.dispatch_forever();
suads 7:bc915651d90e 169 return 0;
suads 7:bc915651d90e 170
suads 7:bc915651d90e 171 }
suads 7:bc915651d90e 172
suads 7:bc915651d90e 173 // int main()
suads 7:bc915651d90e 174 // {
suads 7:bc915651d90e 175 // pc.baud(115200);
suads 7:bc915651d90e 176 // pc.printf("START_1\n");
suads 7:bc915651d90e 177 // mpu6050.calibrate(accelBias, gyroBias);
suads 7:bc915651d90e 178 // mpu6050.initialize();
suads 7:bc915651d90e 179 // pc.printf("START_2\n");
suads 7:bc915651d90e 180
suads 5:ea629a3fd6c1 181
suads 5:ea629a3fd6c1 182
suads 7:bc915651d90e 183 // /////////////////////////////////////////////////////////SERIAL////////////////////////////////////////////
suads 7:bc915651d90e 184 // while(0){
suads 7:bc915651d90e 185 // indicatorLED = 0;
suads 7:bc915651d90e 186 // wait(1);
suads 7:bc915651d90e 187 // if(pc.readable()){
suads 7:bc915651d90e 188 // if(pc.getc()=='.'){
suads 7:bc915651d90e 189 // pc.putc(pc.getc());
suads 7:bc915651d90e 190 // indicatorLED=1;
suads 7:bc915651d90e 191 // wait(1);
suads 7:bc915651d90e 192 // }
suads 7:bc915651d90e 193 // }
suads 5:ea629a3fd6c1 194
suads 7:bc915651d90e 195 // }
suads 5:ea629a3fd6c1 196
suads 7:bc915651d90e 197 // /////////////////////////////////////////////////////////SERIAL/////////////////////////////////////////////
suads 5:ea629a3fd6c1 198
suads 7:bc915651d90e 199 // /////////////////////////////////////////////////////////MPU6050////////////////////////////////////////////
suads 5:ea629a3fd6c1 200
suads 7:bc915651d90e 201 // while(0){
suads 7:bc915651d90e 202 // pc.printf("i am %d\n",mpu6050.getDeviceID());
suads 7:bc915651d90e 203 // wait(1);
suads 7:bc915651d90e 204 // }
suads 5:ea629a3fd6c1 205
suads 7:bc915651d90e 206 // /////////////////////////////////////////////////////////MPU6050/////////////////////////////////////////////
suads 5:ea629a3fd6c1 207
suads 5:ea629a3fd6c1 208
suads 5:ea629a3fd6c1 209
suads 7:bc915651d90e 210 // //////////////////////////////////////////////////////////////GET_DATA////////////////////////////////////////////
suads 7:bc915651d90e 211 // while(1){
suads 5:ea629a3fd6c1 212
suads 7:bc915651d90e 213 // mpu6050.readAccelData(accelData);
suads 7:bc915651d90e 214 // mpu6050.getAres();
suads 7:bc915651d90e 215 // ax = accelData[0]*aRes - accelBias[0];
suads 7:bc915651d90e 216 // ay = accelData[1]*aRes - accelBias[1];
suads 7:bc915651d90e 217 // az = accelData[2]*aRes - accelBias[2];
suads 7:bc915651d90e 218 // float accelValues[3] = {ax,ay,az};
suads 7:bc915651d90e 219 // mpu6050.readGyroData(gyroData);
suads 7:bc915651d90e 220 // mpu6050.getGres();
suads 7:bc915651d90e 221 // gx = gyroData[0]*gRes - gyroBias[0];
suads 7:bc915651d90e 222 // gy = gyroData[1]*gRes - gyroBias[1];
suads 7:bc915651d90e 223 // gz = gyroData[2]*gRes - gyroBias[2];
suads 7:bc915651d90e 224 // float gyroValues[3] = {gx,gy,gz};
suads 5:ea629a3fd6c1 225
suads 7:bc915651d90e 226 // if(pc.readable())
suads 7:bc915651d90e 227 // {
suads 7:bc915651d90e 228 // if(pc.getc()=='.'){
suads 5:ea629a3fd6c1 229
suads 7:bc915651d90e 230 // pc.printf("%f, ",gx);
suads 7:bc915651d90e 231 // pc.printf("%f, ",gy);
suads 7:bc915651d90e 232 // pc.printf("%f",gz);
suads 5:ea629a3fd6c1 233
suads 7:bc915651d90e 234 // }
OsmanKameric 4:147bbe6f9626 235
suads 7:bc915651d90e 236 // }
suads 5:ea629a3fd6c1 237
suads 7:bc915651d90e 238 // }
suads 7:bc915651d90e 239 // }