Tas Lab / Mbed 2 deprecated BLE_Speedsensor

Speed sensor for tasdevelop

Dependencies:   BLE_API MMA8451Q_ss mbed

Fork of FRDM_MMA8451Q by Daniel K

main.cpp@1:6bd72ba7fa56, 2015-02-18 (annotated)

Committer:
suzukimitsuhiro
Date:
Wed Feb 18 08:15:33 2015 +0000
Revision:
1:6bd72ba7fa56
Parent:
0:fa7bb836444d
Child:
2:9d566ea4f4fa
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Daniel90 0:fa7bb836444d 1 #include "mbed.h"
Daniel90 0:fa7bb836444d 2 #include "MMA8451Q.h"
suzukimitsuhiro 1:6bd72ba7fa56 3 #include "BLEDevice.h"
Daniel90 0:fa7bb836444d 4
suzukimitsuhiro 1:6bd72ba7fa56 5 #define MMA8451_I2C_ADDRESS (0x1C<<1)
suzukimitsuhiro 1:6bd72ba7fa56 6
suzukimitsuhiro 1:6bd72ba7fa56 7 #define NEED_CONSOLE_OUTPUT 0 /* Set this if you need debug messages on the console;
suzukimitsuhiro 1:6bd72ba7fa56 8 * it will have an impact on code-size and power consumption. */
suzukimitsuhiro 1:6bd72ba7fa56 9
suzukimitsuhiro 1:6bd72ba7fa56 10 #if NEED_CONSOLE_OUTPUT
suzukimitsuhiro 1:6bd72ba7fa56 11 Serial pc(USBTX, USBRX);
suzukimitsuhiro 1:6bd72ba7fa56 12 #define DEBUG(...) { pc.printf(__VA_ARGS__); }
Daniel90 0:fa7bb836444d 13 #else
suzukimitsuhiro 1:6bd72ba7fa56 14 #define DEBUG(...) /* nothing */
suzukimitsuhiro 1:6bd72ba7fa56 15 #endif /* #if NEED_CONSOLE_OUTPUT */
suzukimitsuhiro 1:6bd72ba7fa56 16
suzukimitsuhiro 1:6bd72ba7fa56 17
suzukimitsuhiro 1:6bd72ba7fa56 18
suzukimitsuhiro 1:6bd72ba7fa56 19 PinName const SDA = p22;
suzukimitsuhiro 1:6bd72ba7fa56 20 PinName const SCL = p20;
suzukimitsuhiro 1:6bd72ba7fa56 21 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
suzukimitsuhiro 1:6bd72ba7fa56 22
suzukimitsuhiro 1:6bd72ba7fa56 23 //Serial pc(USBTX, USBRX);
suzukimitsuhiro 1:6bd72ba7fa56 24 Ticker ticker;
suzukimitsuhiro 1:6bd72ba7fa56 25 Ticker ticker2;
suzukimitsuhiro 1:6bd72ba7fa56 26
suzukimitsuhiro 1:6bd72ba7fa56 27 InterruptIn button(p1);
suzukimitsuhiro 1:6bd72ba7fa56 28
suzukimitsuhiro 1:6bd72ba7fa56 29 const static char DEVICE_NAME[] = "HRM1017_SPEED";
suzukimitsuhiro 1:6bd72ba7fa56 30 static volatile bool triggerSensorPolling = false;
suzukimitsuhiro 1:6bd72ba7fa56 31 BLEDevice ble;
suzukimitsuhiro 1:6bd72ba7fa56 32
suzukimitsuhiro 1:6bd72ba7fa56 33 /* Health Thermometer Service */
suzukimitsuhiro 1:6bd72ba7fa56 34 uint8_t thermTempPayload[5] = { 0, 0, 0, 0, 0 };
suzukimitsuhiro 1:6bd72ba7fa56 35 GattCharacteristic tempChar (GattCharacteristic::UUID_TEMPERATURE_MEASUREMENT_CHAR, thermTempPayload, 5, 5, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE);
suzukimitsuhiro 1:6bd72ba7fa56 36 GattCharacteristic *htmChars[] = {&tempChar, };
suzukimitsuhiro 1:6bd72ba7fa56 37 GattService htmService(GattService::UUID_HEALTH_THERMOMETER_SERVICE, htmChars, sizeof(htmChars) / sizeof(GattCharacteristic *));
suzukimitsuhiro 1:6bd72ba7fa56 38 uint16_t uuid16_list[] = {GattService::UUID_HEALTH_THERMOMETER_SERVICE};
suzukimitsuhiro 1:6bd72ba7fa56 39
suzukimitsuhiro 1:6bd72ba7fa56 40 static Gap::ConnectionParams_t connectionParams;
suzukimitsuhiro 1:6bd72ba7fa56 41
suzukimitsuhiro 1:6bd72ba7fa56 42 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) // Mod
suzukimitsuhiro 1:6bd72ba7fa56 43 {
suzukimitsuhiro 1:6bd72ba7fa56 44 // advertisingStateLed = 1;
suzukimitsuhiro 1:6bd72ba7fa56 45 DEBUG("Disconnected handle %u, reason %u\n", handle, reason);
suzukimitsuhiro 1:6bd72ba7fa56 46 DEBUG("Restarting the advertising process\n\r");
suzukimitsuhiro 1:6bd72ba7fa56 47 ble.startAdvertising();
suzukimitsuhiro 1:6bd72ba7fa56 48 }
suzukimitsuhiro 1:6bd72ba7fa56 49
suzukimitsuhiro 1:6bd72ba7fa56 50 void onConnectionCallback(Gap::Handle_t handle, const Gap::ConnectionParams_t *params) //Mod
suzukimitsuhiro 1:6bd72ba7fa56 51 {
suzukimitsuhiro 1:6bd72ba7fa56 52 // advertisingStateLed = 0;
suzukimitsuhiro 1:6bd72ba7fa56 53 DEBUG("connected. Got handle %u\r\n", handle);
suzukimitsuhiro 1:6bd72ba7fa56 54 connectionParams.slaveLatency = 1;
suzukimitsuhiro 1:6bd72ba7fa56 55 if (ble.updateConnectionParams(handle, &connectionParams) != BLE_ERROR_NONE) {
suzukimitsuhiro 1:6bd72ba7fa56 56 DEBUG("failed to update connection paramter\r\n");
suzukimitsuhiro 1:6bd72ba7fa56 57 }
suzukimitsuhiro 1:6bd72ba7fa56 58 }
suzukimitsuhiro 1:6bd72ba7fa56 59 void GetAccelterData( void );
suzukimitsuhiro 1:6bd72ba7fa56 60 void periodicCallback(void)
suzukimitsuhiro 1:6bd72ba7fa56 61 {
suzukimitsuhiro 1:6bd72ba7fa56 62 // DEBUG("periodicCallback\n\r");
suzukimitsuhiro 1:6bd72ba7fa56 63 // oneSecondLed = !oneSecondLed; /* Do blinky on LED1 while we're waiting for BLE events */
suzukimitsuhiro 1:6bd72ba7fa56 64 GetAccelterData();
suzukimitsuhiro 1:6bd72ba7fa56 65 triggerSensorPolling = true;
suzukimitsuhiro 1:6bd72ba7fa56 66
suzukimitsuhiro 1:6bd72ba7fa56 67 }
suzukimitsuhiro 1:6bd72ba7fa56 68
suzukimitsuhiro 1:6bd72ba7fa56 69
suzukimitsuhiro 1:6bd72ba7fa56 70 void initBLE( void )
suzukimitsuhiro 1:6bd72ba7fa56 71 {
suzukimitsuhiro 1:6bd72ba7fa56 72
suzukimitsuhiro 1:6bd72ba7fa56 73 ticker.attach_us(periodicCallback, 10000); //10msecの周期タイマー
suzukimitsuhiro 1:6bd72ba7fa56 74
suzukimitsuhiro 1:6bd72ba7fa56 75 ble.init();
suzukimitsuhiro 1:6bd72ba7fa56 76 DEBUG("Init done\n");
suzukimitsuhiro 1:6bd72ba7fa56 77 ble.onDisconnection(disconnectionCallback);
suzukimitsuhiro 1:6bd72ba7fa56 78 ble.onConnection(onConnectionCallback);
suzukimitsuhiro 1:6bd72ba7fa56 79
suzukimitsuhiro 1:6bd72ba7fa56 80 ble.getPreferredConnectionParams(&connectionParams);
Daniel90 0:fa7bb836444d 81
suzukimitsuhiro 1:6bd72ba7fa56 82 /* setup advertising */
suzukimitsuhiro 1:6bd72ba7fa56 83 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
suzukimitsuhiro 1:6bd72ba7fa56 84 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
suzukimitsuhiro 1:6bd72ba7fa56 85 ble.accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_THERMOMETER);
suzukimitsuhiro 1:6bd72ba7fa56 86 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
suzukimitsuhiro 1:6bd72ba7fa56 87 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
suzukimitsuhiro 1:6bd72ba7fa56 88 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ //Advertisingの周期時間 0.625msec単位
suzukimitsuhiro 1:6bd72ba7fa56 89 ble.startAdvertising();
suzukimitsuhiro 1:6bd72ba7fa56 90 // advertisingStateLed = 1;
suzukimitsuhiro 1:6bd72ba7fa56 91 DEBUG("Start Advertising\n");
suzukimitsuhiro 1:6bd72ba7fa56 92
suzukimitsuhiro 1:6bd72ba7fa56 93 ble.addService(htmService);
suzukimitsuhiro 1:6bd72ba7fa56 94
suzukimitsuhiro 1:6bd72ba7fa56 95
suzukimitsuhiro 1:6bd72ba7fa56 96 }
suzukimitsuhiro 1:6bd72ba7fa56 97 //-------------------------------------
suzukimitsuhiro 1:6bd72ba7fa56 98
suzukimitsuhiro 1:6bd72ba7fa56 99 float ox=0;
suzukimitsuhiro 1:6bd72ba7fa56 100 float oy=0;
suzukimitsuhiro 1:6bd72ba7fa56 101 float oz=0;
suzukimitsuhiro 1:6bd72ba7fa56 102 float dx=0;
suzukimitsuhiro 1:6bd72ba7fa56 103 float dy=0;
suzukimitsuhiro 1:6bd72ba7fa56 104 float dz=0;
suzukimitsuhiro 1:6bd72ba7fa56 105 bool f=1;
suzukimitsuhiro 1:6bd72ba7fa56 106 float velocity = 0;
suzukimitsuhiro 1:6bd72ba7fa56 107 float maxvelocity = 0;
suzukimitsuhiro 1:6bd72ba7fa56 108 void GetAccelterData( void )
suzukimitsuhiro 1:6bd72ba7fa56 109 {
suzukimitsuhiro 1:6bd72ba7fa56 110 // DEBUG("aaaaa\r\n");
suzukimitsuhiro 1:6bd72ba7fa56 111 float x = acc.getAccX() * 9.8;
suzukimitsuhiro 1:6bd72ba7fa56 112 float y = acc.getAccY() * 9.8;
suzukimitsuhiro 1:6bd72ba7fa56 113 float z = acc.getAccZ() * 9.8;
suzukimitsuhiro 1:6bd72ba7fa56 114
suzukimitsuhiro 1:6bd72ba7fa56 115 if(f==0){
suzukimitsuhiro 1:6bd72ba7fa56 116 dx+=(x-ox);
suzukimitsuhiro 1:6bd72ba7fa56 117 dy+=(y-oy);
suzukimitsuhiro 1:6bd72ba7fa56 118 dz+=(z-oz);
suzukimitsuhiro 1:6bd72ba7fa56 119 }
suzukimitsuhiro 1:6bd72ba7fa56 120 f=0;
suzukimitsuhiro 1:6bd72ba7fa56 121 // float v = sqrt(dx*dx+dy*dy+dz*dz) * 3600/1000/100;
suzukimitsuhiro 1:6bd72ba7fa56 122 float v = abs(dx+dy+dz) * 3600/1000/10/3;
suzukimitsuhiro 1:6bd72ba7fa56 123
suzukimitsuhiro 1:6bd72ba7fa56 124 // pc.printf("X: %7.2f, Y: %7.2f, Z: %7.2f v:%7.2f\r\n", x-ox, y-oy, z-oz, v);
suzukimitsuhiro 1:6bd72ba7fa56 125 // pc.printf("X: %7.2f, Y: %7.2f, Z: %7.2f v:%7.2f\r\n", dx, dy, dz, v);
suzukimitsuhiro 1:6bd72ba7fa56 126 // pc.printf("X: %7.2f, Y: %7.2f, Z: %7.2f v:%7.2f\r\n", x, y, z, v);
suzukimitsuhiro 1:6bd72ba7fa56 127
suzukimitsuhiro 1:6bd72ba7fa56 128 velocity = v;
suzukimitsuhiro 1:6bd72ba7fa56 129 if (maxvelocity < velocity){
suzukimitsuhiro 1:6bd72ba7fa56 130 maxvelocity = velocity;
suzukimitsuhiro 1:6bd72ba7fa56 131 }
suzukimitsuhiro 1:6bd72ba7fa56 132
suzukimitsuhiro 1:6bd72ba7fa56 133 // if(v>1.0f){
suzukimitsuhiro 1:6bd72ba7fa56 134 DEBUG("%7.2f km/h Max:%7.2f km/h (?)\r\n", v ,maxvelocity);
suzukimitsuhiro 1:6bd72ba7fa56 135 // }
suzukimitsuhiro 1:6bd72ba7fa56 136
suzukimitsuhiro 1:6bd72ba7fa56 137
suzukimitsuhiro 1:6bd72ba7fa56 138 ox=x;
suzukimitsuhiro 1:6bd72ba7fa56 139 oy=y;
suzukimitsuhiro 1:6bd72ba7fa56 140 oz=z;
suzukimitsuhiro 1:6bd72ba7fa56 141 }
suzukimitsuhiro 1:6bd72ba7fa56 142 void InitAccelterData( void )
suzukimitsuhiro 1:6bd72ba7fa56 143 {
suzukimitsuhiro 1:6bd72ba7fa56 144 DEBUG("MMA8451 ID: %d\r\n", acc.getWhoAmI());
suzukimitsuhiro 1:6bd72ba7fa56 145 // Ticker ticker;
suzukimitsuhiro 1:6bd72ba7fa56 146 // ticker2.attach_us(GetAccelterData, 10000); //10000usec = 10msecの周期タイマー
suzukimitsuhiro 1:6bd72ba7fa56 147 // ticker2.attach(GetAccelterData, 1); //10000usec = 10msecの周期タイマー
suzukimitsuhiro 1:6bd72ba7fa56 148 }
suzukimitsuhiro 1:6bd72ba7fa56 149 void onButton( void )
suzukimitsuhiro 1:6bd72ba7fa56 150 {
suzukimitsuhiro 1:6bd72ba7fa56 151 DEBUG("#####\r\n");
suzukimitsuhiro 1:6bd72ba7fa56 152 dx=0;
suzukimitsuhiro 1:6bd72ba7fa56 153 dy=0;
suzukimitsuhiro 1:6bd72ba7fa56 154 dz=0;
suzukimitsuhiro 1:6bd72ba7fa56 155 maxvelocity = 0;
suzukimitsuhiro 1:6bd72ba7fa56 156 }
suzukimitsuhiro 1:6bd72ba7fa56 157
suzukimitsuhiro 1:6bd72ba7fa56 158 void updateServiceValues(void);
Daniel90 0:fa7bb836444d 159
Daniel90 0:fa7bb836444d 160 int main(void) {
suzukimitsuhiro 1:6bd72ba7fa56 161 #if NEED_CONSOLE_OUTPUT
suzukimitsuhiro 1:6bd72ba7fa56 162 pc.baud(115200);
suzukimitsuhiro 1:6bd72ba7fa56 163 #endif
suzukimitsuhiro 1:6bd72ba7fa56 164 DEBUG("tttttt\r\n");
suzukimitsuhiro 1:6bd72ba7fa56 165
suzukimitsuhiro 1:6bd72ba7fa56 166 button.rise(&onButton);
suzukimitsuhiro 1:6bd72ba7fa56 167
suzukimitsuhiro 1:6bd72ba7fa56 168 InitAccelterData();
suzukimitsuhiro 1:6bd72ba7fa56 169 initBLE();
Daniel90 0:fa7bb836444d 170
Daniel90 0:fa7bb836444d 171 while (true) {
suzukimitsuhiro 1:6bd72ba7fa56 172 if (triggerSensorPolling) {
suzukimitsuhiro 1:6bd72ba7fa56 173 triggerSensorPolling = false;
suzukimitsuhiro 1:6bd72ba7fa56 174 updateServiceValues();
suzukimitsuhiro 1:6bd72ba7fa56 175 // DEBUG("----------\r\n");
suzukimitsuhiro 1:6bd72ba7fa56 176 } else {
suzukimitsuhiro 1:6bd72ba7fa56 177 ble.waitForEvent();
suzukimitsuhiro 1:6bd72ba7fa56 178 }
suzukimitsuhiro 1:6bd72ba7fa56 179
Daniel90 0:fa7bb836444d 180 }
Daniel90 0:fa7bb836444d 181 }
suzukimitsuhiro 1:6bd72ba7fa56 182
suzukimitsuhiro 1:6bd72ba7fa56 183 uint32_t quick_ieee11073_from_float(float temperature)
suzukimitsuhiro 1:6bd72ba7fa56 184 {
suzukimitsuhiro 1:6bd72ba7fa56 185 uint8_t exponent = 0xFF; //exponent is -1
suzukimitsuhiro 1:6bd72ba7fa56 186 uint32_t mantissa = (uint32_t)(temperature*10);
suzukimitsuhiro 1:6bd72ba7fa56 187
suzukimitsuhiro 1:6bd72ba7fa56 188 return ( ((uint32_t)exponent) << 24) | mantissa;
suzukimitsuhiro 1:6bd72ba7fa56 189 }
suzukimitsuhiro 1:6bd72ba7fa56 190
suzukimitsuhiro 1:6bd72ba7fa56 191 void updateServiceValues(void)
suzukimitsuhiro 1:6bd72ba7fa56 192 {
suzukimitsuhiro 1:6bd72ba7fa56 193
suzukimitsuhiro 1:6bd72ba7fa56 194 uint32_t temp_ieee11073 = quick_ieee11073_from_float(maxvelocity);
suzukimitsuhiro 1:6bd72ba7fa56 195 memcpy(thermTempPayload+1, &temp_ieee11073, 4);
suzukimitsuhiro 1:6bd72ba7fa56 196 ble.updateCharacteristicValue(tempChar.getValueAttribute().getHandle(), thermTempPayload, sizeof(thermTempPayload)); //Mod
suzukimitsuhiro 1:6bd72ba7fa56 197 }
suzukimitsuhiro 1:6bd72ba7fa56 198
suzukimitsuhiro 1:6bd72ba7fa56 199
suzukimitsuhiro 1:6bd72ba7fa56 200
suzukimitsuhiro 1:6bd72ba7fa56 201