Suad Suljic / Mbed OS a

seismo

Dependencies:   SDFileSystem circular_buffer MPU6050 SoftSerial

source/main.cpp@3:b2087af18efe, 2017-06-21 (annotated)

Committer:
OsmanKameric
Date:
Wed Jun 21 16:00:36 2017 +0000
Revision:
3:b2087af18efe
Parent:
1:2cc1c9a45be7
Child:
4:147bbe6f9626
-MPU6050 update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OsmanKameric 0:a4de55cab4e2 1 #include <events/mbed_events.h>
OsmanKameric 0:a4de55cab4e2 2 #include <mbed.h>
OsmanKameric 0:a4de55cab4e2 3 #include "ble/BLE.h"
OsmanKameric 0:a4de55cab4e2 4 #include "ble/Gap.h"
OsmanKameric 0:a4de55cab4e2 5 #include "ble/services/BatteryService.h"
OsmanKameric 0:a4de55cab4e2 6 #include "MPU6050.h"
OsmanKameric 0:a4de55cab4e2 7 #include "MPUService.h"
OsmanKameric 1:2cc1c9a45be7 8 #include <string>
OsmanKameric 1:2cc1c9a45be7 9 #include <stdio.h>
OsmanKameric 3:b2087af18efe 10 #include "SoftSerial.h"
OsmanKameric 3:b2087af18efe 11 Serial pc(p6, p8);
OsmanKameric 3:b2087af18efe 12 //SoftSerial pc1(p9, p10);
OsmanKameric 0:a4de55cab4e2 13 MPU6050 mpu6050;
OsmanKameric 3:b2087af18efe 14 //InterruptIn pin(p10);
OsmanKameric 3:b2087af18efe 15 //DigitalOut led(LED1, 1);
OsmanKameric 3:b2087af18efe 16 //DigitalOut led2(LED2, 1);
OsmanKameric 1:2cc1c9a45be7 17 uint16_t customServiceUUID = 0xA000;
OsmanKameric 1:2cc1c9a45be7 18 uint16_t readCharUUID = 0xA001;
OsmanKameric 3:b2087af18efe 19 uint16_t interruptUUID = 0xA002;
OsmanKameric 1:2cc1c9a45be7 20 DigitalOut alivenessLED(LED1, 0);
OsmanKameric 0:a4de55cab4e2 21 const static char DEVICE_NAME[] = "MPU";
OsmanKameric 1:2cc1c9a45be7 22 static const uint16_t uuid16_list[] = {0x0A0A};
OsmanKameric 3:b2087af18efe 23 //static int counter;
OsmanKameric 1:2cc1c9a45be7 24 /* Set Up custom Characteristics */
OsmanKameric 1:2cc1c9a45be7 25 static float readValue[3] = {0};
OsmanKameric 3:b2087af18efe 26 static float interruptValue[3] = {0};
OsmanKameric 1:2cc1c9a45be7 27 ReadOnlyArrayGattCharacteristic<float, sizeof(readValue)> readChar(readCharUUID, readValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
OsmanKameric 3:b2087af18efe 28 ReadOnlyArrayGattCharacteristic<float, sizeof(interruptValue)> interrupt(interruptUUID, interruptValue, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
OsmanKameric 1:2cc1c9a45be7 29
OsmanKameric 1:2cc1c9a45be7 30 //static float writeValue[2] = {3,4};
OsmanKameric 1:2cc1c9a45be7 31 //WriteOnlyArrayGattCharacteristic<float, sizeof(writeValue)> writeChar(writeCharUUID, writeValue);
OsmanKameric 1:2cc1c9a45be7 32
OsmanKameric 1:2cc1c9a45be7 33 /* Set up custom service */
OsmanKameric 3:b2087af18efe 34 GattCharacteristic *characteristics[] = {&readChar/*, &writeChar*/, &interrupt};
OsmanKameric 1:2cc1c9a45be7 35 GattService customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));
OsmanKameric 1:2cc1c9a45be7 36
OsmanKameric 1:2cc1c9a45be7 37
OsmanKameric 1:2cc1c9a45be7 38
OsmanKameric 0:a4de55cab4e2 39 static EventQueue eventQueue(/* event count */ 10 * /* event size */ 32);
OsmanKameric 3:b2087af18efe 40 InterruptIn button(BUTTON1);
OsmanKameric 3:b2087af18efe 41
OsmanKameric 3:b2087af18efe 42
OsmanKameric 3:b2087af18efe 43
OsmanKameric 3:b2087af18efe 44
OsmanKameric 1:2cc1c9a45be7 45 void updateReadValue() {
OsmanKameric 1:2cc1c9a45be7 46 alivenessLED = !alivenessLED;
OsmanKameric 3:b2087af18efe 47 mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
OsmanKameric 3:b2087af18efe 48
OsmanKameric 1:2cc1c9a45be7 49
OsmanKameric 1:2cc1c9a45be7 50 //mpu6050.readAccelData(accelData);
OsmanKameric 1:2cc1c9a45be7 51 //mpu6050.getAres();
OsmanKameric 1:2cc1c9a45be7 52 //ax = accelData[0] * aRes - accelBias[0];
OsmanKameric 1:2cc1c9a45be7 53 //ay = accelData[1] * aRes - accelBias[1];
OsmanKameric 1:2cc1c9a45be7 54 //az = accelData[2] * aRes - accelBias[2];
OsmanKameric 1:2cc1c9a45be7 55 float newValue[3] = {ax,ay,az};
OsmanKameric 1:2cc1c9a45be7 56 //mpu6050.readGyroData(gyroData);
OsmanKameric 1:2cc1c9a45be7 57 //mpu6050.getGres();
OsmanKameric 1:2cc1c9a45be7 58
OsmanKameric 1:2cc1c9a45be7 59 //gx = gyroData[0] * gRes; // - gyroBias[0];
OsmanKameric 1:2cc1c9a45be7 60 //gy = gyroData[1] * gRes; // - gyroBias[1];
OsmanKameric 1:2cc1c9a45be7 61 //gz = gyroData[2] * gRes; // - gyroBias[2];
OsmanKameric 1:2cc1c9a45be7 62 //float newValue[3] = {gx,gy,gz};*/
OsmanKameric 1:2cc1c9a45be7 63 //counter++;
OsmanKameric 1:2cc1c9a45be7 64 //pc.printf("| Before | ax=%.3f | ay=%.3f | az=%.3f \r\n",gx,gy,gz);
OsmanKameric 3:b2087af18efe 65 pc.printf("| Acce | ax=%.3f | ay=%.3f | az=%.3f | Gyro | gx=%.3f | gy=%.3f | gz=%.3f\r\n",ax,ay,az,gx,gy,gz);
OsmanKameric 1:2cc1c9a45be7 66 //len_send = strlen((const char *)newValue);
OsmanKameric 3:b2087af18efe 67
OsmanKameric 3:b2087af18efe 68
OsmanKameric 3:b2087af18efe 69 uint8_t motionByte = mpu6050.readThisByte(MPU6050_RA_MOT_DETECT_STATUS);
OsmanKameric 3:b2087af18efe 70 pc.printf("Motion byte: %d\r\n",motionByte);
OsmanKameric 3:b2087af18efe 71 BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(interrupt.getValueHandle(), (uint8_t*)motionByte, 8);
OsmanKameric 1:2cc1c9a45be7 72 BLE::Instance(BLE::DEFAULT_INSTANCE).gattServer().write(readChar.getValueHandle(), (uint8_t*)newValue, 12);
OsmanKameric 1:2cc1c9a45be7 73 }
OsmanKameric 1:2cc1c9a45be7 74 void periodicCallback(void)
OsmanKameric 1:2cc1c9a45be7 75 {
OsmanKameric 1:2cc1c9a45be7 76 if (BLE::Instance().getGapState().connected) {
OsmanKameric 1:2cc1c9a45be7 77 eventQueue.call(updateReadValue);
OsmanKameric 1:2cc1c9a45be7 78 }
OsmanKameric 1:2cc1c9a45be7 79 }
OsmanKameric 0:a4de55cab4e2 80
OsmanKameric 0:a4de55cab4e2 81 void scheduleBleEventsProcessing(BLE::OnEventsToProcessCallbackContext* context)
OsmanKameric 0:a4de55cab4e2 82 {
OsmanKameric 0:a4de55cab4e2 83 BLE& ble = BLE::Instance();
OsmanKameric 0:a4de55cab4e2 84 eventQueue.call(Callback<void()>(&ble, &BLE::processEvents));
OsmanKameric 0:a4de55cab4e2 85 }
OsmanKameric 0:a4de55cab4e2 86
OsmanKameric 0:a4de55cab4e2 87 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
OsmanKameric 0:a4de55cab4e2 88 {
OsmanKameric 0:a4de55cab4e2 89 (void) params;
OsmanKameric 0:a4de55cab4e2 90 BLE::Instance().gap().startAdvertising();
OsmanKameric 0:a4de55cab4e2 91 }
OsmanKameric 0:a4de55cab4e2 92 void onBleInitError(BLE &ble, ble_error_t error)
OsmanKameric 0:a4de55cab4e2 93 {
OsmanKameric 0:a4de55cab4e2 94 /* Initialization error handling should go here */
OsmanKameric 0:a4de55cab4e2 95 }
OsmanKameric 1:2cc1c9a45be7 96
OsmanKameric 1:2cc1c9a45be7 97
OsmanKameric 0:a4de55cab4e2 98 void bleInitComplete(BLE::InitializationCompleteCallbackContext* params)
OsmanKameric 0:a4de55cab4e2 99 {
OsmanKameric 0:a4de55cab4e2 100 BLE& ble = params->ble;
OsmanKameric 0:a4de55cab4e2 101 ble_error_t error = params->error;
OsmanKameric 0:a4de55cab4e2 102
OsmanKameric 0:a4de55cab4e2 103 if (error != BLE_ERROR_NONE) {
OsmanKameric 0:a4de55cab4e2 104 /* In case of error, forward the error handling to onBleInitError */
OsmanKameric 0:a4de55cab4e2 105 onBleInitError(ble, error);
OsmanKameric 0:a4de55cab4e2 106 return;
OsmanKameric 0:a4de55cab4e2 107 }
OsmanKameric 0:a4de55cab4e2 108
OsmanKameric 0:a4de55cab4e2 109 /* Ensure that it is the default instance of BLE */
OsmanKameric 0:a4de55cab4e2 110 if (ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
OsmanKameric 0:a4de55cab4e2 111 return;
OsmanKameric 0:a4de55cab4e2 112 }
OsmanKameric 0:a4de55cab4e2 113 ble.gap().onDisconnection(disconnectionCallback);
OsmanKameric 0:a4de55cab4e2 114
OsmanKameric 0:a4de55cab4e2 115 /* setup advertising */
OsmanKameric 0:a4de55cab4e2 116 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
OsmanKameric 0:a4de55cab4e2 117 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
OsmanKameric 0:a4de55cab4e2 118 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t*)DEVICE_NAME, sizeof(DEVICE_NAME));
OsmanKameric 0:a4de55cab4e2 119 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
OsmanKameric 1:2cc1c9a45be7 120
OsmanKameric 1:2cc1c9a45be7 121 ble.gap().startAdvertising();
OsmanKameric 1:2cc1c9a45be7 122 ble.addService(customService);
OsmanKameric 0:a4de55cab4e2 123 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
OsmanKameric 0:a4de55cab4e2 124
OsmanKameric 0:a4de55cab4e2 125
OsmanKameric 0:a4de55cab4e2 126 }
OsmanKameric 1:2cc1c9a45be7 127 void flip() {
OsmanKameric 3:b2087af18efe 128 //pc.printf("Desio se interupt");
OsmanKameric 1:2cc1c9a45be7 129 }
OsmanKameric 1:2cc1c9a45be7 130 bool XnegMD, XposMD, YnegMD, YposMD, ZnegMD, ZposMD;
OsmanKameric 0:a4de55cab4e2 131 int main()
OsmanKameric 1:2cc1c9a45be7 132 {
OsmanKameric 1:2cc1c9a45be7 133 //mpu6050.whoAmI();
OsmanKameric 1:2cc1c9a45be7 134 //mpu6050.calibrate(accelBias, gyroBias);
OsmanKameric 1:2cc1c9a45be7 135 //mpu6050.init();
OsmanKameric 3:b2087af18efe 136 mpu6050.calibrate(accelBias, gyroBias);
OsmanKameric 3:b2087af18efe 137 mpu6050.initialize();
OsmanKameric 3:b2087af18efe 138 mpu6050.setSleepEnabled(0);
OsmanKameric 3:b2087af18efe 139 pc.baud(115200);
OsmanKameric 3:b2087af18efe 140 mpu6050.setDHPFMode(1);
OsmanKameric 1:2cc1c9a45be7 141 //setMotionDetectionThreshold
OsmanKameric 1:2cc1c9a45be7 142 //char myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
OsmanKameric 1:2cc1c9a45be7 143 //pc.printf("Prije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 144 //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
OsmanKameric 1:2cc1c9a45be7 145 //pc.printf("Poslije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 146 //mpu6050.writeByte(MPU6050_ADDRESS, 0x1F,0x02);
OsmanKameric 1:2cc1c9a45be7 147 //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x1F);
OsmanKameric 1:2cc1c9a45be7 148 //pc.printf("MotionDetectionThreshold iznosi: %x\n",myChar);
OsmanKameric 1:2cc1c9a45be7 149
OsmanKameric 1:2cc1c9a45be7 150
OsmanKameric 1:2cc1c9a45be7 151 //setMotionDetectionDuration
OsmanKameric 1:2cc1c9a45be7 152 //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
OsmanKameric 1:2cc1c9a45be7 153 //pc.printf("Prije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 154 //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
OsmanKameric 1:2cc1c9a45be7 155 //pc.printf("Poslije postavljanja\n");
OsmanKameric 1:2cc1c9a45be7 156 //mpu6050.writeByte(MPU6050_ADDRESS, 0x20,0x28);
OsmanKameric 1:2cc1c9a45be7 157 //myChar = mpu6050.readByte(MPU6050_ADDRESS, 0x20);
OsmanKameric 1:2cc1c9a45be7 158 //pc.printf("MotionDetectionDuration iznosi: %x\n",myChar);
OsmanKameric 1:2cc1c9a45be7 159
OsmanKameric 1:2cc1c9a45be7 160 // XnegMD = mpu6050.getXNegMotionDetected();
OsmanKameric 1:2cc1c9a45be7 161 //mpu6050.whoAmI();
OsmanKameric 1:2cc1c9a45be7 162 //uint8_t *data;
OsmanKameric 1:2cc1c9a45be7 163 //char byte = mpu6050.readByte(MPU6050_ADDRESS, 0x61);
OsmanKameric 1:2cc1c9a45be7 164 //*data = byte & (1 << 7);
OsmanKameric 1:2cc1c9a45be7 165 //pc.printf("Byte iznosi: %x\n",byte);
OsmanKameric 1:2cc1c9a45be7 166 //int i = data[0];
OsmanKameric 1:2cc1c9a45be7 167 //pc.printf("7 bit iznosi: %d\n", i);
OsmanKameric 1:2cc1c9a45be7 168
OsmanKameric 1:2cc1c9a45be7 169 //XposMD = mpu6050.getXPosMotionDetected();
OsmanKameric 1:2cc1c9a45be7 170
OsmanKameric 1:2cc1c9a45be7 171
OsmanKameric 1:2cc1c9a45be7 172 //pin.fall(&flip);
OsmanKameric 0:a4de55cab4e2 173 eventQueue.call_every(500, periodicCallback);
OsmanKameric 0:a4de55cab4e2 174 BLE& ble = BLE::Instance();
OsmanKameric 0:a4de55cab4e2 175 ble.onEventsToProcess(scheduleBleEventsProcessing);
OsmanKameric 0:a4de55cab4e2 176 ble.init(bleInitComplete);
OsmanKameric 0:a4de55cab4e2 177 eventQueue.dispatch_forever();
OsmanKameric 3:b2087af18efe 178 while (ble.hasInitialized() == false) { /* spin loop */ }
OsmanKameric 3:b2087af18efe 179
OsmanKameric 3:b2087af18efe 180
OsmanKameric 3:b2087af18efe 181
OsmanKameric 3:b2087af18efe 182
OsmanKameric 3:b2087af18efe 183
OsmanKameric 3:b2087af18efe 184
OsmanKameric 3:b2087af18efe 185 // pc1.baud(9600);
OsmanKameric 3:b2087af18efe 186 //while (true) {
OsmanKameric 3:b2087af18efe 187 //ble.waitForEvent();
OsmanKameric 3:b2087af18efe 188 // pc1.printf("tare ");
OsmanKameric 3:b2087af18efe 189 //wait_ms(1000);
OsmanKameric 3:b2087af18efe 190
OsmanKameric 3:b2087af18efe 191 //}
OsmanKameric 3:b2087af18efe 192 //return 0;
OsmanKameric 0:a4de55cab4e2 193 }