Diff: main.cpp

Revision:

70:c6b61c5cadf4

Parent:

69:12b1170b510a

Child:

71:a6a052fd3d22

--- a/main.cpp	Fri Mar 24 10:58:11 2017 +0100
+++ b/main.cpp	Sun Nov 12 20:48:31 2017 +0000
@@ -54,6 +54,7 @@
 #include "mbed.h"
 #include "assert.h"
 #include "x_nucleo_iks01a1.h"
+#include <math.h>
 
 #include <Ticker.h>
 
@@ -61,6 +62,8 @@
 /*** Constants ---------------------------------------------------------------- ***/
 namespace {
 	const int MS_INTERVALS = 1000;
+	const double RAD_TO_DEG = 57.2957786;
+	const double PI = 3.14159265;
 }
 
 
@@ -82,6 +85,8 @@
     int32_t AXIS_Z;
 } AxesRaw_TypeDef;
 
+/*** Serial declaration --------------------------------------------------------- ***/
+Serial ser(USBTX,USBRX,115200);
 
 /*** Static variables --------------------------------------------------------- ***/
 #ifdef DBG_MCU
@@ -94,10 +99,6 @@
 static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
 static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
 static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;
-static HumiditySensor *humidity_sensor = mems_expansion_board->ht_sensor;;
-static PressureSensor *pressure_sensor = mems_expansion_board->pt_sensor;
-static TempSensor *temp_sensor1 = mems_expansion_board->ht_sensor;
-static TempSensor *temp_sensor2 = mems_expansion_board->pt_sensor;
 
 static Ticker ticker;
 static DigitalOut myled(LED1, LED_OFF);
@@ -174,16 +175,7 @@
 /* Initialization function */
 static void init(void) {
 	uint8_t id1, id2;
-
-	/* Determine ID of Humidity & Temperature Sensor */
-	CALL_METH(humidity_sensor, read_id, &id1, 0x0);
-	CALL_METH(temp_sensor1, read_id, &id2, 0x0);
-    	printf("Humidity  | Temperature Sensor ID = %s (0x%x | 0x%x)\n", 
-	       ((id1 == I_AM_HTS221) ? "HTS221 " : "UNKNOWN"),
-	       id1, id2
-	       );
-	assert(id1 == id2);
-
+	
 	/* Determine ID of Gyro & Motion Sensor */
 	assert((mems_expansion_board->gyro_lsm6ds0 == NULL) ||
 	       (mems_expansion_board->gyro_lsm6ds3 == NULL));
@@ -205,10 +197,6 @@
 
 /* Main cycle function */
 static void main_cycle(void) {
-	float TEMPERATURE_Value;
-	float HUMIDITY_Value;
-	float PRESSURE_Value;
-	float PRESSURE_Temp_Value;
 	AxesRaw_TypeDef MAG_Value;
 	AxesRaw_TypeDef ACC_Value;
 	AxesRaw_TypeDef GYR_Value;
@@ -218,15 +206,16 @@
 	char buffer4[32];
 	unsigned int ret = 0;
 	
+	/* Declaration of sensors variables */
+	double accX,accY,accZ;
+	double gyroX,gyroY,gyroZ;
+
+	
 	/* Switch LED On */
 	myled = LED_ON;
 	printf("===\n");
 
 	/* Determine Environmental Values */
-	ret |= (!CALL_METH(temp_sensor1, get_temperature, &TEMPERATURE_Value, 0.0f) ? 0x0 : 0x1);
-	ret |= (!CALL_METH(humidity_sensor, get_humidity, &HUMIDITY_Value, 0.0f) ? 0x0 : 0x2);;
-	ret |= (!CALL_METH(pressure_sensor, get_pressure, &PRESSURE_Value, 0.0f) ? 0x0 : 0x4);;
-	ret |= (!CALL_METH(temp_sensor2, get_fahrenheit, &PRESSURE_Temp_Value, 0.0f) ? 0x0 : 0x8);;
 	ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
 	ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
 	ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
@@ -239,11 +228,36 @@
 	       ACC_Value.AXIS_X, ACC_Value.AXIS_Y, ACC_Value.AXIS_Z);
         printf("GYR [mdps]:   %9ld %9ld %9ld\n", 
 	       GYR_Value.AXIS_X, GYR_Value.AXIS_Y, GYR_Value.AXIS_Z);
-        printf("---\nTEMP | HUMIDITY: %s°C | %s%%\nTEMP | PRESSURE: %s°F | %smbar\n", 
-	       printDouble(buffer1, TEMPERATURE_Value), 
-	       printDouble(buffer2, HUMIDITY_Value), 
-	       printDouble(buffer4, PRESSURE_Temp_Value),
-	       printDouble(buffer3, PRESSURE_Value));
+	       
+	accX = ACC_Value.AXIS_X;
+  	accY = ACC_Value.AXIS_Y;
+  	accZ = ACC_Value.AXIS_Z;
+  	gyroX = GYR_Value.AXIS_X;
+  	gyroY = GYR_Value.AXIS_Y;
+  	gyroZ = GYR_Value.AXIS_Z;
+  	
+  	#ifdef RESTRICT_PITCH // Eq. 25 and 26
+  	double roll  = atan2(accY, accZ) * RAD_TO_DEG;
+  	double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
+	#else // Eq. 28 and 29
+	double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
+  	double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
+	#endif
+	double yaw = atan2(-accZ, sqrt(accY * accY + accZ * accZ)) * 180.0/PI;
+	
+	/* Print Serially *//*
+	ser.printf("%lf",pitch);
+	ser.printf(":");
+	ser.printf("%lf",roll);
+	ser.printf(":");
+	ser.printf("%lf\n",yaw);
+	*/
+	ser.printf("1");
+	ser.printf(":");
+	ser.printf("2");
+	ser.printf(":");
+	ser.printf("3\n");
+	
 	
 	/* Switch LED Off */
 	myled = LED_OFF;