Diff: main.cpp

Revision:

18:9b9ec0b35b45

Parent:

13:fc873da5b445

Child:

19:29d3c7caea66

--- a/main.cpp	Wed Sep 27 15:48:21 2017 +0000
+++ b/main.cpp	Tue Nov 19 00:42:38 2019 +0000
@@ -4,7 +4,7 @@
  * @author  CLab
  * @version V1.0.0
  * @date    2-December-2016
- * @brief   Simple Example application for using the X_NUCLEO_IKS01A1 
+ * @brief   Simple Example application for using the X_NUCLEO_IKS01A1
  *          MEMS Inertial & Environmental Sensor Nucleo expansion board.
  ******************************************************************************
  * @attention
@@ -34,111 +34,157 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
-*/ 
+*/
 
 /* Includes */
 #include "mbed.h"
 #include "XNucleoIKS01A2.h"
+#include <iostream>
+#include <cmath>
 
 /* Instantiate the expansion board */
 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
 
 /* Retrieve the composing elements of the expansion board */
 static LSM303AGRMagSensor *magnetometer = mems_expansion_board->magnetometer;
-static HTS221Sensor *hum_temp = mems_expansion_board->ht_sensor;
-static LPS22HBSensor *press_temp = mems_expansion_board->pt_sensor;
 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
 static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
 
-/* Helper function for printing floats & doubles */
-static char *print_double(char* str, double v, int decimalDigits=2)
-{
-  int i = 1;
-  int intPart, fractPart;
-  int len;
-  char *ptr;
+
+
+
+
+struct Vec3 {
+    float x, y, z;
+};
+
+
+static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f};
 
-  /* prepare decimal digits multiplicator */
-  for (;decimalDigits!=0; i*=10, decimalDigits--);
+Vec3 get_gyro()
+{
+    Vec3 vec3;
 
-  /* calculate integer & fractinal parts */
-  intPart = (int)v;
-  fractPart = (int)((v-(double)(int)v)*i);
+    int32_t axes[3];
+    acc_gyro->get_g_axes(axes);
 
-  /* fill in integer part */
-  sprintf(str, "%i.", intPart);
+    vec3.x = axes[0]/1000.0f - gyro_calibrate.x;
+    vec3.y = axes[1]/1000.0f - gyro_calibrate.y;
+    vec3.z = axes[2]/1000.0f - gyro_calibrate.z;
 
-  /* prepare fill in of fractional part */
-  len = strlen(str);
-  ptr = &str[len];
+    return vec3;
+}
+
+Vec3 get_accel()
+{
+    Vec3 vec3;
+    int32_t axes[3];
+    acc_gyro->get_x_axes(axes);
 
-  /* fill in leading fractional zeros */
-  for (i/=10;i>1; i/=10, ptr++) {
-    if (fractPart >= i) {
-      break;
-    }
-    *ptr = '0';
-  }
+    vec3.x = axes[0]/1000.0f + 0.013f;
+    vec3.y = axes[1]/1000.0f + 0.004f;
+    vec3.z = axes[2]/1000.0f - 0.032f;
+
+    return vec3;
+}
 
-  /* fill in (rest of) fractional part */
-  sprintf(ptr, "%i", fractPart);
+
 
-  return str;
+void print_vec3(const char* str, const Vec3& vec3)
+{
+    std::cout << str << vec3.x << " " << vec3.y << " " << vec3.z << "\r\n";
 }
 
+
 /* Simple main function */
-int main() {
-  uint8_t id;
-  float value1, value2;
-  char buffer1[32], buffer2[32];
-  int32_t axes[3];
-  
-  /* Enable all sensors */
-  hum_temp->enable();
-  press_temp->enable();
-  magnetometer->enable();
-  accelerometer->enable();
-  acc_gyro->enable_x();
-  acc_gyro->enable_g();
-  
-  printf("\r\n--- Starting new run ---\r\n");
+int main()
+{
+
+    /* Enable all sensors */
+    magnetometer->enable();
+    accelerometer->enable();
+    acc_gyro->enable_x();
+    acc_gyro->enable_g();
+
+    //Timer t;
+    //float dt = 0.02f;
+
+    std::cout << "\r\n--- Starting new run ---\r\n";
+
+    //Vec3 orientation = {};
+
+    //float sens = 0.488f;
+    
+    Vec3 vec3, tilt;
+    
+    // 180/pi -> converts radians to degrees.
+    float rad_to_deg = 57.2957795131f;
+
+
+    for(;;) {
+        //t.start();
+
+
+        //Vec3 gyro_reading = get_gyro();
+
+        //orientation.x += (gyro_reading.x * dt * sens);
+        //orientation.y += (gyro_reading.y * dt * sens);
+        //orientation.z += (gyro_reading.z * dt * sens);
+
+        //print_vec3("orientation: ", orientation);
+        
+        
+        
+        vec3 = get_accel();
+        
+        tilt.x = atan(vec3.x / sqrtf(vec3.y*vec3.y + vec3.z*vec3.z)) * rad_to_deg;
+        tilt.y = atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)) * rad_to_deg;
+        tilt.z = atan(sqrtf(vec3.y*vec3.y + vec3.x*vec3.x) / vec3.z) * rad_to_deg;
+
+
 
-  hum_temp->read_id(&id);
-  printf("HTS221  humidity & temperature    = 0x%X\r\n", id);
-  press_temp->read_id(&id);
-  printf("LPS22HB  pressure & temperature   = 0x%X\r\n", id);
-  magnetometer->read_id(&id);
-  printf("LSM303AGR magnetometer            = 0x%X\r\n", id);
-  accelerometer->read_id(&id);
-  printf("LSM303AGR accelerometer           = 0x%X\r\n", id);
-  acc_gyro->read_id(&id);
-  printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
- 
-  while(1) {
-    printf("\r\n");
+
+        print_vec3("acceleration: ", tilt);
+
+        std::cout << "\r\n" << std::flush;
+
+        t.stop();
+        if (dt - t.read() > 0) wait(dt - t.read());
+        dt = t.read();
+    }
+
+
+
+
+
+
+
+
+
+    //for(;;) {
+    //-0.42 -1.61 1.05
+    //magnetometer->get_m_axes(axes);
+    //std::cout << "LSM303AGR [mag/gauss]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
 
-    hum_temp->get_temperature(&value1);
-    hum_temp->get_humidity(&value2);
-    printf("HTS221: [temp] %7s C,   [hum] %s%%\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
-    
-    press_temp->get_temperature(&value1);
-    press_temp->get_pressure(&value2);
-    printf("LPS22HB: [temp] %7s C, [press] %s mbar\r\n", print_double(buffer1, value1), print_double(buffer2, value2));
+    //accelerometer->get_x_axes(axes);
+    //std::cout << "LSM303AGR [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
+
 
-    printf("---\r\n");
+    //acc_gyro->get_x_axes(axes);
+    //std::cout << "LSM6DSL [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n";
+
+
+    //acc_gyro->get_g_axes(axes);
+    //std::cout << "LSM6DSL [gyro/dps]: " << axes[0]/1000.0f + 0.42f << " " << axes[1]/1000.0f + 1.61f << " " << axes[2]/1000.0f-1.05f << "\r\n";
 
-    magnetometer->get_m_axes(axes);
-    printf("LSM303AGR [mag/mgauss]:  %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-    
-    accelerometer->get_x_axes(axes);
-    printf("LSM303AGR [acc/mg]:  %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
+
+
+
 
-    acc_gyro->get_x_axes(axes);
-    printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
+    //print_vec3("gyro/dps: ", get_gyro());
+    //std::cout << "\r\n" << std::flush;
+
 
-    acc_gyro->get_g_axes(axes);
-    printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-
-    wait(1.5);
-  }
+    //wait(1.5);
+    //}
 }