Sensors.cpp

00001 #include "Sensors.h"
00002 
00003 
00004 using namespace std;
00005 
00006 void enable_sensors()
00007 {
00008   /* Init all sensors with default params */
00009   press_s.init(NULL);
00010   acc_gyro.init(NULL);
00011   range.init_sensor(VL53L0X_DEFAULT_ADDRESS);
00012 
00013   /* Enable all sensors */
00014   press_s.enable();
00015   acc_gyro.enable_x();
00016   acc_gyro.enable_g();
00017 }
00018 
00019 void get_values()
00020 {
00021   press_status = 0;
00022 
00023   //Temperature sensor
00024   temp = hum_temp_s.getCelsius();
00025 
00026   //Humidity sensor
00027   hum = hum_temp_s.getHumidity();
00028 
00029 
00030 
00031   //Light sensor
00032   light = light_s.read();
00033 
00034   //Pressure sensor
00035   if(press_s.get_pressure(&press) != 0)
00036     {
00037       printf("Error getting pressure\r\n");
00038       press_status = VALUE_ERROR;
00039     }
00040 
00041 }
00042 
00043 void get_motion_data(int dt, int* speed, int* prev_speed, int* prev_pos, int* prev_angle)
00044 {
00045 
00046   //Accelerometer
00047   if(acc_gyro.get_x_axes(acc_axes) != 0)
00048     {
00049       printf("Error getting acceleration\n");
00050     }
00051 
00052   //Gyroscope
00053   if(acc_gyro.get_g_axes(gyro_axes) != 0)
00054     {
00055       printf("Error getting data from gyroscope\r\n");
00056     }
00057 
00058   prev_speed[0] += acc_axes[0]*dt;
00059   prev_speed[1] += acc_axes[1]*dt;
00060   //prev_speed[2] += acc_axes[2]*dt;
00061 
00062   *speed = (int)sqrt(pow(prev_speed[0],2.0)+pow(prev_speed[1],2.0)); //+pow(prev_speed[2],2.0)); //restrain to 2D
00063 
00064   prev_pos[0] += prev_speed[0]*dt; //x
00065   prev_pos[1] += prev_speed[1]*dt; //y
00066 
00067   *prev_angle += gyro_axes[2]*dt; //rotation around z axis
00068 
00069 }