main.cpp

00001 #include "mbed.h"
00002 #include "BLEDevice.h"
00003 #include "ble_hts.h"
00004 #include "LSM9DS0.h"
00005 
00006 #define LSM9DS0_XM  0x1E // Would be 0x1E if SDO_XM is LOW
00007 #define LSM9DS0_G   0x6A // Would be 0x6A if SDO_G is LOW
00008 
00009 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
00010 * it will have an impact on code-size and power consumption. */
00011 #if NEED_CONSOLE_OUTPUT
00012 Serial  pc(USBTX, USBRX);
00013 #define DEBUG(...) { pc.printf(__VA_ARGS__); }
00014 #else
00015 #define DEBUG(...) /* nothing */
00016 #endif /* #if NEED_CONSOLE_OUTPUT */
00017 
00018 const static char  DEVICE_NAME[] = "HRM1017_HTM";
00019 
00020 BLEDevice  ble;
00021 LSM9DS0 dof(p22, p20, LSM9DS0_G, LSM9DS0_XM);
00022 
00023 //Bluetooth LE
00024 uint8_t             thermTempPayload[12] = { 0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0};
00025 GattCharacteristic  tempChar (GattCharacteristic::UUID_TEMPERATURE_MEASUREMENT_CHAR,thermTempPayload, 12, 12,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE);
00026 GattCharacteristic *htmChars[] = {&tempChar, };
00027 GattService        htmService(GattService::UUID_HEALTH_THERMOMETER_SERVICE, htmChars,sizeof(htmChars) / sizeof(GattCharacteristic *));
00028 uint16_t             uuid16_list[] = {GattService::UUID_HEALTH_THERMOMETER_SERVICE};
00029 uint32_t quick_ieee11073_from_float(float temperature);
00030 static Gap::ConnectionParams_t connectionParams;
00031 
00032 //LSM9DS0
00033 //float beta;             // algorithm gain
00034 //float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
00035 //float deltat = 0.0f;
00036 //uint32_t lastUpdate = 0;
00037 //uint32_t Now = 0;
00038 //Timer t;
00039 void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
00040 
00041 void updateServiceValues(void);
00042 
00043 void disconnectionCallback(Gap::Handle_t handle)
00044 {
00045     ble.startAdvertising();
00046 }
00047 
00048 void onConnectionCallback(Gap::Handle_t handle)
00049 {
00050     connectionParams.slaveLatency = 1;
00051     if (ble.updateConnectionParams(handle, &connectionParams) != BLE_ERROR_NONE) {
00052         //not found
00053     }
00054 }
00055 
00056 void periodicCallback(void)
00057 {
00058     ble.updateCharacteristicValue(tempChar.getHandle(), thermTempPayload, sizeof(thermTempPayload));
00059     ble.waitForEvent();
00060 }
00061 
00062 /**************************************************************************/
00063 /*!
00064     @brief  Program entry point
00065 */
00066 /**************************************************************************/
00067 int main(void)
00068 {
00069     //t.start();
00070 
00071     //beta = sqrt(3.0f / 4.0f) * 3.14f * (40.0f / 180.0f);
00072 
00073     dof.begin();
00074     //dof.calLSM9DS0(dof.gbias, dof.abias);
00075 
00076     ble.init();
00077     ble.onDisconnection(disconnectionCallback);
00078     ble.onConnection(onConnectionCallback);
00079 
00080     ble.getPreferredConnectionParams(&connectionParams);
00081 
00082     /* setup advertising */
00083     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
00084     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
00085     ble.accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_THERMOMETER);
00086     //Set Device Name
00087     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
00088     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00089     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
00090     ble.startAdvertising();
00091 
00092     ble.addService(htmService);
00093     //Change nRF51822/projectconfig.h L118,L119 500_50 1000_100
00094 
00095     Ticker ticker;
00096     ticker.attach(periodicCallback, 0.1);
00097 
00098     for (;;) {
00099         updateServiceValues();
00100     }
00101 }
00102 
00103 void updateServiceValues(void)
00104 {
00105     //dof.readGyro();
00106     //float gx = dof.calcGyro(dof.gx) - dof.gbias[0];
00107     //float gy = dof.calcGyro(dof.gy) - dof.gbias[1];
00108     //float gz = dof.calcGyro(dof.gz) - dof.gbias[2];
00109     dof.readMag();
00110     //float mx = dof.calcMag(dof.mx);
00111     //float my = dof.calcMag(dof.my);
00112     //float mz = dof.calcMag(dof.mz);
00113     dof.readAccel();
00114     //float ax = dof.calcAccel(dof.ax) - dof.abias[0];
00115     //float ay = dof.calcAccel(dof.ay) - dof.abias[1];
00116     //float az = dof.calcAccel(dof.az) - dof.abias[2];
00117     //MadgwickAHRSupdate(ax, ay, az, gx*3.14f/180.0f, gy*3.14f/180.0f, gz*3.14f/180.0f, mx, my, mz);
00118 
00119     //Now = t.read_us();
00120     //deltat = ((Now - lastUpdate)/1000000.0f);
00121     //lastUpdate = Now;
00122 
00123     //float yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
00124     //float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00125     //float roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
00126     //pitch *= 180.0f / 3.14;
00127     //yaw   *= 180.0f / 3.14;
00128     //yaw   -= 13.8;
00129     //roll  *= 180.0f / 3.14;
00130 
00131     float yaw,roll,pitch;
00132     float x=dof.calcAccel(dof.ax);
00133     float y=dof.calcAccel(dof.ay);
00134     float z=dof.calcAccel(dof.az);
00135     pitch = atan2(x, sqrt((y * y) + (z * z)));
00136     roll = atan2(y, sqrt((x * x) + (z * z)));
00137     pitch *= 180.0 / 3.14;
00138     roll *= 180.0 / 3.14;
00139     //
00140     if (dof.my > 0) {
00141         yaw = 90 - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
00142     } else if (dof.my < 0) {
00143         yaw = - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
00144     } else { // dof.my = 0
00145         if (dof.mx < 0) yaw = 180;
00146         else yaw = 0;
00147     }
00148 
00149     yaw = atan2(z, sqrt((x * x) + (y * y)));
00150     yaw *= 180.0 / 3.14;
00151 
00152     memcpy(thermTempPayload, &pitch, 4);
00153     memcpy(thermTempPayload+4, &roll, 4);
00154     memcpy(thermTempPayload+8, &yaw, 4);
00155 }
00156 
00157 /*void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
00158   float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
00159   float norm;
00160   float hx, hy, _2bx, _2bz;
00161   float s1, s2, s3, s4;
00162   float qDot1, qDot2, qDot3, qDot4;
00163 
00164   // Auxiliary variables to avoid repeated arithmetic
00165   float _2q1mx;
00166   float _2q1my;
00167   float _2q1mz;
00168   float _2q2mx;
00169   float _4bx;
00170   float _4bz;
00171   float _2q1 = 2.0f * q1;
00172   float _2q2 = 2.0f * q2;
00173   float _2q3 = 2.0f * q3;
00174   float _2q4 = 2.0f * q4;
00175   float _2q1q3 = 2.0f * q1 * q3;
00176   float _2q3q4 = 2.0f * q3 * q4;
00177   float q1q1 = q1 * q1;
00178   float q1q2 = q1 * q2;
00179   float q1q3 = q1 * q3;
00180   float q1q4 = q1 * q4;
00181   float q2q2 = q2 * q2;
00182   float q2q3 = q2 * q3;
00183   float q2q4 = q2 * q4;
00184   float q3q3 = q3 * q3;
00185   float q3q4 = q3 * q4;
00186   float q4q4 = q4 * q4;
00187 
00188   // Normalise accelerometer measurement
00189   norm = sqrt(ax * ax + ay * ay + az * az);
00190   if (norm == 0.0f) return; // handle NaN
00191   norm = 1.0f/norm;
00192   ax *= norm;
00193   ay *= norm;
00194   az *= norm;
00195 
00196   // Normalise magnetometer measurement
00197   norm = sqrt(mx * mx + my * my + mz * mz);
00198   if (norm == 0.0f) return; // handle NaN
00199   norm = 1.0f/norm;
00200   mx *= norm;
00201   my *= norm;
00202   mz *= norm;
00203 
00204   // Reference direction of Earth's magnetic field
00205   _2q1mx = 2.0f * q1 * mx;
00206   _2q1my = 2.0f * q1 * my;
00207   _2q1mz = 2.0f * q1 * mz;
00208   _2q2mx = 2.0f * q2 * mx;
00209   hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
00210   hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
00211   _2bx = sqrt(hx * hx + hy * hy);
00212   _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
00213   _4bx = 2.0f * _2bx;
00214   _4bz = 2.0f * _2bz;
00215 
00216   // Gradient decent algorithm corrective step
00217   s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00218   s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00219   s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00220   s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
00221   norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
00222   norm = 1.0f/norm;
00223   s1 *= norm;
00224   s2 *= norm;
00225   s3 *= norm;
00226   s4 *= norm;
00227 
00228   // Compute rate of change of quaternion
00229   qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
00230   qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
00231   qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
00232   qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
00233 
00234   // Integrate to yield quaternion
00235   q1 += qDot1 * deltat;
00236   q2 += qDot2 * deltat;
00237   q3 += qDot3 * deltat;
00238   q4 += qDot4 * deltat;
00239   norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
00240   norm = 1.0f/norm;
00241   q[0] = q1 * norm;
00242   q[1] = q2 * norm;
00243   q[2] = q3 * norm;
00244   q[3] = q4 * norm;
00245 }*/