Revision 2:3822b8c8bbe2, committed 2015-11-03

Comitter:

akihiro0105

Date:

Tue Nov 03 13:59:07 2015 +0000

Parent:

1:50056f53e2ac

Child:

3:49052c8d8e3c

Commit message:

20151103

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS0.lib	Tue Nov 03 13:59:07 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/randrews33/code/LSM9DS0/#635b453fa29a

--- a/TMP102.lib	Fri Aug 22 11:32:14 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/chris/code/TMP102/#694792b93731

--- a/main.cpp	Fri Aug 22 11:32:14 2014 +0000
+++ b/main.cpp	Tue Nov 03 13:59:07 2015 +0000
@@ -1,11 +1,13 @@
 #include "mbed.h"
-#include "TMP102.h"
 #include "BLEDevice.h"
 #include "ble_hts.h"
+#include "LSM9DS0.h"
+
+#define LSM9DS0_XM  0x1E // Would be 0x1E if SDO_XM is LOW
+#define LSM9DS0_G   0x6A // Would be 0x6A if SDO_G is LOW
 
 #define NEED_CONSOLE_OUTPUT 1 /* Set this if you need debug messages on the console;
                                * it will have an impact on code-size and power consumption. */
-
 #if NEED_CONSOLE_OUTPUT
 Serial  pc(USBTX, USBRX);
 #define DEBUG(...) { pc.printf(__VA_ARGS__); }
@@ -16,61 +18,47 @@
 const static char  DEVICE_NAME[] = "HRM1017_HTM";
 
 BLEDevice  ble;
-TMP102      healthThemometer(p22, p20, 0x90);  /* The TMP102 connected to our board */
-
-/* LEDs for indication: */
-DigitalOut  oneSecondLed(LED1);        /* LED1 is toggled every second. */
-DigitalOut  advertisingStateLed(LED2); /* LED2 is on when we are advertising, otherwise off. */
-
-
-/* Health Thermometer Service */ 
-uint8_t             thermTempPayload[5] = { 0, 0, 0, 0, 0 };
+LSM9DS0 dof(p22, p20, LSM9DS0_G, LSM9DS0_XM);
 
-GattCharacteristic  tempChar (GattCharacteristic::UUID_TEMPERATURE_MEASUREMENT_CHAR,
-                                thermTempPayload, 5, 5,
-                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE);
-/* Battery Level Service */
-uint8_t            batt = 100;     /* Battery level */
-uint8_t            read_batt = 0;  /* Variable to hold battery level reads */
-GattCharacteristic battLevel ( GattCharacteristic::UUID_BATTERY_LEVEL_CHAR, 
-                                 (uint8_t *)batt, 1, 1,
-                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
+//Bluetooth LE
+uint8_t             thermTempPayload[12] = { 0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0};
+GattCharacteristic  tempChar (GattCharacteristic::UUID_TEMPERATURE_MEASUREMENT_CHAR,thermTempPayload, 12, 12,GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_INDICATE);
 GattCharacteristic *htmChars[] = {&tempChar, };
-GattCharacteristic *battChars[] = {&battLevel, };
-GattService        htmService(GattService::UUID_HEALTH_THERMOMETER_SERVICE, htmChars, 
-                                sizeof(htmChars) / sizeof(GattCharacteristic *));
-GattService        battService(GattService::UUID_BATTERY_SERVICE, battChars,
-                                sizeof(battChars) / sizeof(GattCharacteristic *));
+GattService        htmService(GattService::UUID_HEALTH_THERMOMETER_SERVICE, htmChars,sizeof(htmChars) / sizeof(GattCharacteristic *));
+uint16_t             uuid16_list[] = {GattService::UUID_HEALTH_THERMOMETER_SERVICE};
+uint32_t quick_ieee11073_from_float(float temperature);
+static Gap::ConnectionParams_t connectionParams;
 
-uint16_t             uuid16_list[] = {GattService::UUID_HEALTH_THERMOMETER_SERVICE,
-                                      GattService::UUID_BATTERY_SERVICE};
+//LSM9DS0
+//float beta;             // algorithm gain
+//float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};
+//float deltat = 0.0f;
+//uint32_t lastUpdate = 0;
+//uint32_t Now = 0;
+//Timer t;
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
 
-uint32_t quick_ieee11073_from_float(float temperature);
 void updateServiceValues(void);
 
-static Gap::ConnectionParams_t connectionParams;
-
 void disconnectionCallback(Gap::Handle_t handle)
-{
-    advertisingStateLed = 1;
-    
-    DEBUG("Disconnected handle %u!\n\r", handle);
-    DEBUG("Restarting the advertising process\n\r");
+{   
     ble.startAdvertising();
 }
 
 void onConnectionCallback(Gap::Handle_t handle)
 {
-    advertisingStateLed = 0;
-
-    DEBUG("connected. Got handle %u\r\n", handle);
-
     connectionParams.slaveLatency = 1;
     if (ble.updateConnectionParams(handle, &connectionParams) != BLE_ERROR_NONE) {
-        DEBUG("failed to update connection paramter\r\n");
+        //not found
     }
 }
 
+void periodicCallback(void)
+{
+    ble.updateCharacteristicValue(tempChar.getHandle(), thermTempPayload, sizeof(thermTempPayload));
+    ble.waitForEvent();
+}
+
 /**************************************************************************/
 /*!
     @brief  Program entry point
@@ -78,11 +66,13 @@
 /**************************************************************************/
 int main(void)
 {
+    //t.start();
     
-    /* Setup blinky led */
-    oneSecondLed = 1;
+    //beta = sqrt(3.0f / 4.0f) * 3.14f * (40.0f / 180.0f);
     
-    DEBUG("Initialising the nRF51822\n\r");
+    dof.begin();
+    //dof.calLSM9DS0(dof.gbias, dof.abias);
+    
     ble.init();
     ble.onDisconnection(disconnectionCallback);
     ble.onConnection(onConnectionCallback);
@@ -93,56 +83,169 @@
     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t*)uuid16_list, sizeof(uuid16_list));
     ble.accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_THERMOMETER);
+    //Set Device Name
     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
     ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
     ble.startAdvertising();
-    advertisingStateLed = 1;
 
     ble.addService(htmService);
-    ble.addService(battService);
-
+    //Change nRF51822/projectconfig.h L118,L119 500_50 1000_100
+    
+    Ticker ticker;
+    ticker.attach(periodicCallback, 0.1);
+    
     for (;;)
     {
-        /* Now that we're live, update the battery level & temperature characteristics */
         updateServiceValues();
-        wait(1);
-        ble.waitForEvent();
     }
 }
 
-/**************************************************************************/
-/*!
-    @brief  Ticker callback to switch advertisingStateLed state
-*/
-/**************************************************************************/
 void updateServiceValues(void)
 {
-      /* Toggle the one second LEDs */
-      oneSecondLed = !oneSecondLed;
+      //dof.readGyro();
+      //float gx = dof.calcGyro(dof.gx) - dof.gbias[0];
+      //float gy = dof.calcGyro(dof.gy) - dof.gbias[1];
+      //float gz = dof.calcGyro(dof.gz) - dof.gbias[2];
+      dof.readMag();
+      //float mx = dof.calcMag(dof.mx);
+      //float my = dof.calcMag(dof.my);
+      //float mz = dof.calcMag(dof.mz);
+      dof.readAccel();
+      //float ax = dof.calcAccel(dof.ax) - dof.abias[0];
+      //float ay = dof.calcAccel(dof.ay) - dof.abias[1];
+      //float az = dof.calcAccel(dof.az) - dof.abias[2];
+      //MadgwickAHRSupdate(ax, ay, az, gx*3.14f/180.0f, gy*3.14f/180.0f, gz*3.14f/180.0f, mx, my, mz);
       
-      /* Decrement the battery level. */
-      batt <=50 ? batt=100 : batt--;
+      //Now = t.read_us();
+      //deltat = ((Now - lastUpdate)/1000000.0f);
+      //lastUpdate = Now;
+      
+      //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]);   
+      //float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+      //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]);
+      //pitch *= 180.0f / 3.14;
+      //yaw   *= 180.0f / 3.14; 
+      //yaw   -= 13.8;
+      //roll  *= 180.0f / 3.14;
       
-      /* Update the temperature. Note that we need to convert to an ieee11073 format float. */
-      float temperature = healthThemometer.read();
-      DEBUG("temp:%f\n", temperature);
-      uint32_t temp_ieee11073 = quick_ieee11073_from_float(temperature);
-      memcpy(thermTempPayload+1, &temp_ieee11073, 4);
-      ble.updateCharacteristicValue(tempChar.getHandle(), thermTempPayload, sizeof(thermTempPayload));
-      ble.updateCharacteristicValue(battLevel.getHandle(), (uint8_t *)&batt, sizeof(batt));
+      float yaw,roll,pitch;
+      float x=dof.calcAccel(dof.ax);
+      float y=dof.calcAccel(dof.ay);
+      float z=dof.calcAccel(dof.az);
+      pitch = atan2(x, sqrt((y * y) + (z * z)));
+      roll = atan2(y, sqrt((x * x) + (z * z)));
+      pitch *= 180.0 / 3.14;
+      roll *= 180.0 / 3.14;
+      //
+      if (dof.my > 0)
+      {
+          yaw = 90 - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
+      }
+      else if (dof.my < 0)
+      {
+          yaw = - (atan((float)dof.mx / (float)dof.my) * (180 / 3.14));
+      }
+      else // dof.my = 0
+      {
+          if (dof.mx < 0) yaw = 180;
+          else yaw = 0;
+      }
+      
+      yaw = atan2(z, sqrt((x * x) + (y * y)));
+      yaw *= 180.0 / 3.14;
+      
+      memcpy(thermTempPayload, &pitch, 4);
+      memcpy(thermTempPayload+4, &roll, 4);
+      memcpy(thermTempPayload+8, &yaw, 4);
 }
 
-/**
- * @brief A very quick conversion between a float temperature and 11073-20601 FLOAT-Type.
- * @param temperature The temperature as a float.
- * @return The temperature in 11073-20601 FLOAT-Type format.
- */
-uint32_t quick_ieee11073_from_float(float temperature)
-{
-    uint8_t  exponent = 0xFF; //exponent is -1
-    uint32_t mantissa = (uint32_t)(temperature*10);
-    
-    return ( ((uint32_t)exponent) << 24) | mantissa;
-}
+/*void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+  float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
+  float norm;
+  float hx, hy, _2bx, _2bz;
+  float s1, s2, s3, s4;
+  float qDot1, qDot2, qDot3, qDot4;
+
+  // Auxiliary variables to avoid repeated arithmetic
+  float _2q1mx;
+  float _2q1my;
+  float _2q1mz;
+  float _2q2mx;
+  float _4bx;
+  float _4bz;
+  float _2q1 = 2.0f * q1;
+  float _2q2 = 2.0f * q2;
+  float _2q3 = 2.0f * q3;
+  float _2q4 = 2.0f * q4;
+  float _2q1q3 = 2.0f * q1 * q3;
+  float _2q3q4 = 2.0f * q3 * q4;
+  float q1q1 = q1 * q1;
+  float q1q2 = q1 * q2;
+  float q1q3 = q1 * q3;
+  float q1q4 = q1 * q4;
+  float q2q2 = q2 * q2;
+  float q2q3 = q2 * q3;
+  float q2q4 = q2 * q4;
+  float q3q3 = q3 * q3;
+  float q3q4 = q3 * q4;
+  float q4q4 = q4 * q4;
+
+  // Normalise accelerometer measurement
+  norm = sqrt(ax * ax + ay * ay + az * az);
+  if (norm == 0.0f) return; // handle NaN
+  norm = 1.0f/norm;
+  ax *= norm;
+  ay *= norm;
+  az *= norm;
 
+  // Normalise magnetometer measurement
+  norm = sqrt(mx * mx + my * my + mz * mz);
+  if (norm == 0.0f) return; // handle NaN
+  norm = 1.0f/norm;
+  mx *= norm;
+  my *= norm;
+  mz *= norm;
+
+  // Reference direction of Earth's magnetic field
+  _2q1mx = 2.0f * q1 * mx;
+  _2q1my = 2.0f * q1 * my;
+  _2q1mz = 2.0f * q1 * mz;
+  _2q2mx = 2.0f * q2 * mx;
+  hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+  hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+  _2bx = sqrt(hx * hx + hy * hy);
+  _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+  _4bx = 2.0f * _2bx;
+  _4bz = 2.0f * _2bz;
+
+  // Gradient decent algorithm corrective step
+  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);
+  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);
+  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);
+  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);
+  norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
+  norm = 1.0f/norm;
+  s1 *= norm;
+  s2 *= norm;
+  s3 *= norm;
+  s4 *= norm;
+
+  // Compute rate of change of quaternion
+  qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
+  qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
+  qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
+  qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
+
+  // Integrate to yield quaternion
+  q1 += qDot1 * deltat;
+  q2 += qDot2 * deltat;
+  q3 += qDot3 * deltat;
+  q4 += qDot4 * deltat;
+  norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+  norm = 1.0f/norm;
+  q[0] = q1 * norm;
+  q[1] = q2 * norm;
+  q[2] = q3 * norm;
+  q[3] = q4 * norm;
+}*/

--- a/nRF51822.lib	Fri Aug 22 11:32:14 2014 +0000
+++ b/nRF51822.lib	Tue Nov 03 13:59:07 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/todotani/code/nRF51822/#f3badab88f02
+http://mbed.org/users/todotani/code/nRF51822/#82cfbf5035b1