Diff: Sensors.cpp

Revision:

0:07ff689741d2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors.cpp	Fri Jun 08 12:00:02 2018 +0000
@@ -0,0 +1,69 @@
+#include "Sensors.h"
+
+
+using namespace std;
+
+void enable_sensors()
+{
+  /* Init all sensors with default params */
+  press_s.init(NULL);
+  acc_gyro.init(NULL);
+  range.init_sensor(VL53L0X_DEFAULT_ADDRESS);
+
+  /* Enable all sensors */
+  press_s.enable();
+  acc_gyro.enable_x();
+  acc_gyro.enable_g();
+}
+
+void get_values()
+{
+  press_status = 0;
+
+  //Temperature sensor
+  temp = hum_temp_s.getCelsius();
+
+  //Humidity sensor
+  hum = hum_temp_s.getHumidity();
+
+
+
+  //Light sensor
+  light = light_s.read();
+
+  //Pressure sensor
+  if(press_s.get_pressure(&press) != 0)
+    {
+      printf("Error getting pressure\r\n");
+      press_status = VALUE_ERROR;
+    }
+
+}
+
+void get_motion_data(int dt, int* speed, int* prev_speed, int* prev_pos, int* prev_angle)
+{
+
+  //Accelerometer
+  if(acc_gyro.get_x_axes(acc_axes) != 0)
+    {
+      printf("Error getting acceleration\n");
+    }
+
+  //Gyroscope
+  if(acc_gyro.get_g_axes(gyro_axes) != 0)
+    {
+      printf("Error getting data from gyroscope\r\n");
+    }
+
+  prev_speed[0] += acc_axes[0]*dt;
+  prev_speed[1] += acc_axes[1]*dt;
+  //prev_speed[2] += acc_axes[2]*dt;
+
+  *speed = (int)sqrt(pow(prev_speed[0],2.0)+pow(prev_speed[1],2.0)); //+pow(prev_speed[2],2.0)); //restrain to 2D
+
+  prev_pos[0] += prev_speed[0]*dt; //x
+  prev_pos[1] += prev_speed[1]*dt; //y
+
+  *prev_angle += gyro_axes[2]*dt; //rotation around z axis
+
+}