Srishti Gaihre / Mbed OS acnSENSA

Degree Computation

Dependencies:   aconno_SEGGER_RTT LSM9DS1 Si7006A20 adc52832_common aconnoMPL115A1 aconno_bsp

aconnoHelpers/aconnoHelpers.cpp@5:6d20ad2a69ba, 2019-01-08 (annotated)

Committer:
jurica238814
Date:
Tue Jan 08 13:15:30 2019 +0000
Revision:
5:6d20ad2a69ba
Parent:
4:cb3513aa9814 
Small changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jurica238814 0:442d98af8cc7 1 /**
jurica238814 0:442d98af8cc7 2 * Made by Jurica @ aconno
jurica238814 0:442d98af8cc7 3 * jurica@aconno.de
jurica238814 0:442d98af8cc7 4 * More info @ aconno.de
jurica238814 0:442d98af8cc7 5 */
jurica238814 0:442d98af8cc7 6
jurica238814 0:442d98af8cc7 7 #include "mbed.h"
jurica238814 0:442d98af8cc7 8 #include "ble/BLE.h"
jurica238814 0:442d98af8cc7 9 #include "acd52832_bsp.h"
jurica238814 0:442d98af8cc7 10 #include "GapAdvertisingData.h"
jurica238814 0:442d98af8cc7 11 #include "Si7006A20.h"
jurica238814 0:442d98af8cc7 12 #include "LSM9DS1.h"
jurica238814 0:442d98af8cc7 13 #include "math.h"
jurica238814 0:442d98af8cc7 14 #include "adc52832_common/utilities.h"
jurica238814 0:442d98af8cc7 15 #include "MPL115A1.h"
jurica238814 0:442d98af8cc7 16 #include "acd_nrf52_saadc.h"
jurica238814 0:442d98af8cc7 17 #include "aconnoConfig.h"
Srishti12 4:cb3513aa9814 18 #include <iostream>
Srishti12 4:cb3513aa9814 19 #include <cmath>
jurica238814 0:442d98af8cc7 20
jurica238814 0:442d98af8cc7 21 NRF52_SAADC analogIn;
jurica238814 0:442d98af8cc7 22 extern DigitalOut lightPower;
jurica238814 0:442d98af8cc7 23 extern DigitalOut temperaturePower;
jurica238814 0:442d98af8cc7 24 extern DigitalOut shdn;
jurica238814 0:442d98af8cc7 25 extern DigitalOut power;
jurica238814 0:442d98af8cc7 26 vector3_s memsAccelerometerInit;
jurica238814 0:442d98af8cc7 27 vector3_s memsGyroscopeInit;
jurica238814 0:442d98af8cc7 28 vector3_s memsMagnetometerInit;
jurica238814 0:442d98af8cc7 29 extern GapAdvertisingData adv_data;
jurica238814 0:442d98af8cc7 30 extern advertising_packet advertisementPacket;
jurica238814 0:442d98af8cc7 31 DigitalOut cs(p7);
jurica238814 0:442d98af8cc7 32
jurica238814 3:b62a73dc76b7 33 I2C i2c(I2C_DATA, I2C_CLK);
jurica238814 3:b62a73dc76b7 34 Si7006 si(&i2c);
jurica238814 3:b62a73dc76b7 35 SPI spi(SPI_MOSI, SPI_MISO, SPI_SCLK);
jurica238814 3:b62a73dc76b7 36 MPL115A1 mpl115a1(spi, cs);
jurica238814 3:b62a73dc76b7 37 LSM9DS1 mems(&i2c);
jurica238814 3:b62a73dc76b7 38
jurica238814 0:442d98af8cc7 39 float getLight(){
jurica238814 0:442d98af8cc7 40 return ((float)analogIn.getData()[1])/ADC_RESOLUTION * VALUE_TO_PERCENTAGE;
jurica238814 0:442d98af8cc7 41 }
jurica238814 0:442d98af8cc7 42
jurica238814 0:442d98af8cc7 43 float voltage2temp(float vOut){
jurica238814 0:442d98af8cc7 44 return ((float)vOut - (float)V0)/((float)TC);
jurica238814 0:442d98af8cc7 45 }
jurica238814 0:442d98af8cc7 46
jurica238814 0:442d98af8cc7 47 float getTemperature(){
jurica238814 0:442d98af8cc7 48 return voltage2temp(((float)analogIn.getData()[2])/ADC_RESOLUTION * (float)VCC);
jurica238814 0:442d98af8cc7 49 }
jurica238814 0:442d98af8cc7 50
jurica238814 0:442d98af8cc7 51 uint8_t getBattery(){
jurica238814 0:442d98af8cc7 52 uint16_t batteryVoltage = analogIn.getData()[0];
jurica238814 0:442d98af8cc7 53
jurica238814 0:442d98af8cc7 54 const uint16_t zero_percent_limit = 739;
jurica238814 0:442d98af8cc7 55 const uint16_t onehundred_percent_limit = 810;
jurica238814 0:442d98af8cc7 56 const uint16_t percentage_increments = 5;
jurica238814 0:442d98af8cc7 57 uint8_t percentage;
jurica238814 0:442d98af8cc7 58
jurica238814 0:442d98af8cc7 59 if (batteryVoltage < zero_percent_limit)
jurica238814 0:442d98af8cc7 60 {
jurica238814 0:442d98af8cc7 61 percentage = 0;
jurica238814 0:442d98af8cc7 62 }
jurica238814 0:442d98af8cc7 63 else if(batteryVoltage > onehundred_percent_limit)
jurica238814 0:442d98af8cc7 64 {
jurica238814 0:442d98af8cc7 65 percentage = 100;
jurica238814 0:442d98af8cc7 66 }
jurica238814 0:442d98af8cc7 67 else
jurica238814 0:442d98af8cc7 68 {
jurica238814 0:442d98af8cc7 69 batteryVoltage -= zero_percent_limit;
jurica238814 0:442d98af8cc7 70 percentage = (batteryVoltage*100)/(onehundred_percent_limit - zero_percent_limit);
jurica238814 0:442d98af8cc7 71 percentage = percentage/percentage_increments*percentage_increments;
jurica238814 0:442d98af8cc7 72 }
jurica238814 0:442d98af8cc7 73
jurica238814 0:442d98af8cc7 74 return percentage;
jurica238814 0:442d98af8cc7 75 }
jurica238814 0:442d98af8cc7 76
jurica238814 0:442d98af8cc7 77 float getHumidity(){
jurica238814 0:442d98af8cc7 78 float result;
jurica238814 3:b62a73dc76b7 79 si.getHumidity(&result);
jurica238814 0:442d98af8cc7 80 return result;
jurica238814 0:442d98af8cc7 81 }
jurica238814 0:442d98af8cc7 82
jurica238814 0:442d98af8cc7 83 void readGyroscope(vector3_s *gyroscopeData){
jurica238814 3:b62a73dc76b7 84 mems.readGyroscope((int16_t *)gyroscopeData);
jurica238814 0:442d98af8cc7 85 *gyroscopeData -= memsGyroscopeInit;
jurica238814 0:442d98af8cc7 86 }
jurica238814 0:442d98af8cc7 87
jurica238814 0:442d98af8cc7 88 void readAccelerometer(vector3_s *accelerometerData){
jurica238814 3:b62a73dc76b7 89 mems.readAccelerometer((int16_t *)accelerometerData);
jurica238814 0:442d98af8cc7 90 *accelerometerData -= memsAccelerometerInit;
jurica238814 0:442d98af8cc7 91 }
jurica238814 0:442d98af8cc7 92
jurica238814 0:442d98af8cc7 93 void readMagnetometer(vector3_s *magnetometerData){
jurica238814 3:b62a73dc76b7 94 mems.readMagnetometer((int16_t *)magnetometerData);
jurica238814 0:442d98af8cc7 95 *magnetometerData -= memsMagnetometerInit;
jurica238814 0:442d98af8cc7 96 }
jurica238814 0:442d98af8cc7 97
jurica238814 0:442d98af8cc7 98 void calibrateAccelerometer(){
jurica238814 0:442d98af8cc7 99 vector3_s accelerometerData;
jurica238814 0:442d98af8cc7 100 for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
jurica238814 0:442d98af8cc7 101 readAccelerometer(&accelerometerData);
jurica238814 0:442d98af8cc7 102 memsAccelerometerInit += accelerometerData;
jurica238814 0:442d98af8cc7 103 }
jurica238814 0:442d98af8cc7 104 memsAccelerometerInit /= CALIBRATION_STEPS;
jurica238814 0:442d98af8cc7 105 }
jurica238814 0:442d98af8cc7 106
jurica238814 0:442d98af8cc7 107 void calibrateGyroscope(){
jurica238814 0:442d98af8cc7 108 vector3_s gyroscopeData;
jurica238814 0:442d98af8cc7 109 for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
jurica238814 0:442d98af8cc7 110 readGyroscope(&gyroscopeData);
jurica238814 0:442d98af8cc7 111 memsGyroscopeInit += gyroscopeData;
jurica238814 0:442d98af8cc7 112 }
jurica238814 0:442d98af8cc7 113 memsGyroscopeInit /= CALIBRATION_STEPS;
jurica238814 0:442d98af8cc7 114 }
jurica238814 0:442d98af8cc7 115
jurica238814 0:442d98af8cc7 116 void calibrateMag(){
jurica238814 0:442d98af8cc7 117 vector3_s magnetometerData;
jurica238814 0:442d98af8cc7 118 for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){
jurica238814 0:442d98af8cc7 119 readMagnetometer(&magnetometerData);
jurica238814 0:442d98af8cc7 120 memsMagnetometerInit += magnetometerData;
jurica238814 0:442d98af8cc7 121 }
jurica238814 0:442d98af8cc7 122 memsMagnetometerInit /= CALIBRATION_STEPS;
jurica238814 0:442d98af8cc7 123 }
jurica238814 0:442d98af8cc7 124
Srishti12 4:cb3513aa9814 125 float calculateDegree(float accX, float accY, float accZ){
Srishti12 4:cb3513aa9814 126
Srishti12 4:cb3513aa9814 127 float degree;
Srishti12 4:cb3513aa9814 128 if (accX != 0.0f && accY != 0.0f && accZ != 0.0f) {
Srishti12 4:cb3513aa9814 129
Srishti12 4:cb3513aa9814 130 float dnum = sqrt(accY * accY + accZ * accZ);
Srishti12 4:cb3513aa9814 131 degree = atan(accX / dnum);
Srishti12 4:cb3513aa9814 132
Srishti12 4:cb3513aa9814 133 if ((accX < 0 && accY >= 0)) {
Srishti12 4:cb3513aa9814 134 degree = 360 + degree;
Srishti12 4:cb3513aa9814 135 }
Srishti12 4:cb3513aa9814 136 if (accX >= 0 && accY < 0)
Srishti12 4:cb3513aa9814 137 degree = 180 - degree;
Srishti12 4:cb3513aa9814 138
Srishti12 4:cb3513aa9814 139 if (accX < 0 && accY < 0)
Srishti12 4:cb3513aa9814 140 degree = 180 - degree;
Srishti12 4:cb3513aa9814 141
Srishti12 4:cb3513aa9814 142 //cout<<"degree: " << degree;
Srishti12 4:cb3513aa9814 143
Srishti12 4:cb3513aa9814 144 }
Srishti12 4:cb3513aa9814 145 return degree;
Srishti12 4:cb3513aa9814 146
Srishti12 4:cb3513aa9814 147 }
jurica238814 0:442d98af8cc7 148 void updateData()
jurica238814 0:442d98af8cc7 149 {
Srishti12 4:cb3513aa9814 150 //static uint8_t advertisementType = 0;
jurica238814 0:442d98af8cc7 151 int16_t temp_acc[3];
jurica238814 0:442d98af8cc7 152 BLE &ble = BLE::Instance();
jurica238814 0:442d98af8cc7 153
jurica238814 0:442d98af8cc7 154 float result;
Srishti12 4:cb3513aa9814 155 mems.startAccelerometer();
Srishti12 4:cb3513aa9814 156 // advertisementPacket.type = 0x00;
Srishti12 4:cb3513aa9814 157 //Added for Angular sensa
Srishti12 4:cb3513aa9814 158 readAccelerometer((vector3_s *)temp_acc);
Srishti12 4:cb3513aa9814 159 float degreeValue = calculateDegree( (float) temp_acc[1],
Srishti12 4:cb3513aa9814 160 (float) temp_acc[0],
Srishti12 4:cb3513aa9814 161 (float) temp_acc[2]);
Srishti12 4:cb3513aa9814 162
Srishti12 4:cb3513aa9814 163 advertisementPacket.degree = degreeValue;
Srishti12 4:cb3513aa9814 164
Srishti12 4:cb3513aa9814 165 /*if(advertisementType < 1)
jurica238814 3:b62a73dc76b7 166 {
jurica238814 3:b62a73dc76b7 167 power = 1;
jurica238814 3:b62a73dc76b7 168 mems.startAccelerometer();
jurica238814 3:b62a73dc76b7 169 mems.startGyroscope();
jurica238814 3:b62a73dc76b7 170 mems.startMagnetometer();
jurica238814 3:b62a73dc76b7 171 wait_ms(WAKEUP_TIME_DELAY_MS);
jurica238814 3:b62a73dc76b7 172 advertisementPacket.type = 0x00;
jurica238814 3:b62a73dc76b7 173 readGyroscope((vector3_s *)advertisementPacket.gyroscope);
jurica238814 3:b62a73dc76b7 174 readAccelerometer((vector3_s *)temp_acc);
jurica238814 3:b62a73dc76b7 175 readMagnetometer((vector3_s *)advertisementPacket.magnetometer);
Srishti12 4:cb3513aa9814 176 //advertisementPacket.acc_lsb_value = (0xF9E);//(0x3D80);
Srishti12 4:cb3513aa9814 177 //advertisementPacket.accelerometer[0] = temp_acc[1];
Srishti12 4:cb3513aa9814 178 //advertisementPacket.accelerometer[1] = temp_acc[0];
Srishti12 4:cb3513aa9814 179 //advertisementPacket.accelerometer[2] = temp_acc[2];
Srishti12 4:cb3513aa9814 180
Srishti12 4:cb3513aa9814 181 float degreeValue = calculateDegree( (float) temp_acc[1],
Srishti12 4:cb3513aa9814 182 (float) temp_acc[0],
Srishti12 4:cb3513aa9814 183 (float) temp_acc[2]);
Srishti12 4:cb3513aa9814 184
Srishti12 4:cb3513aa9814 185 cout<<"degree: " << degreeValue;
Srishti12 4:cb3513aa9814 186 advertisementPacket.degree = degreeValue;
jurica238814 3:b62a73dc76b7 187 // ^--- That's in ug cuz MSB is 1
jurica238814 3:b62a73dc76b7 188 }
jurica238814 3:b62a73dc76b7 189 else
jurica238814 3:b62a73dc76b7 190 {
jurica238814 3:b62a73dc76b7 191 NRF_SAADC->ENABLE = 1;
jurica238814 3:b62a73dc76b7 192 power = 1;
jurica238814 3:b62a73dc76b7 193 temperaturePower = 1;
jurica238814 3:b62a73dc76b7 194 lightPower = 1;
jurica238814 3:b62a73dc76b7 195 shdn = 1;
jurica238814 3:b62a73dc76b7 196 analogIn.addChannel(9); // Set VDD as source to SAADC
jurica238814 3:b62a73dc76b7 197 analogIn.addChannel(6); // Light
jurica238814 3:b62a73dc76b7 198 analogIn.addChannel(7); // Temp
jurica238814 3:b62a73dc76b7 199 analogIn.calibrate();
jurica238814 3:b62a73dc76b7 200 wait_ms(WAKEUP_TIME_DELAY_MS);
jurica238814 3:b62a73dc76b7 201 analogIn.updateData();
jurica238814 3:b62a73dc76b7 202 wait_ms(WAKEUP_TIME_DELAY_MS);
jurica238814 0:442d98af8cc7 203
jurica238814 3:b62a73dc76b7 204 advertisementPacket.type = 0x01;
jurica238814 3:b62a73dc76b7 205 advertisementPacket.temperature = getTemperature();
jurica238814 3:b62a73dc76b7 206 advertisementPacket.light = getLight();
jurica238814 3:b62a73dc76b7 207 advertisementPacket.humidity = getHumidity();
jurica238814 3:b62a73dc76b7 208 advertisementPacket.pressure = mpl115a1.getPressure();
jurica238814 3:b62a73dc76b7 209 advertisementPacket.battery = getBattery();
jurica238814 3:b62a73dc76b7 210 }
Srishti12 4:cb3513aa9814 211 */
jurica238814 0:442d98af8cc7 212 SPI dummySpi(SPI_MOSI, SPI_MISO, SPI_SCLK);
jurica238814 0:442d98af8cc7 213 power = 0;
jurica238814 0:442d98af8cc7 214 shdn = 0;
jurica238814 0:442d98af8cc7 215 temperaturePower = 0;
jurica238814 0:442d98af8cc7 216 lightPower = 0;
jurica238814 0:442d98af8cc7 217 NRF_SAADC->ENABLE = 0;
jurica238814 0:442d98af8cc7 218
Srishti12 4:cb3513aa9814 219 //if(++advertisementType > 2) advertisementType = 0;
jurica238814 0:442d98af8cc7 220 adv_data = ble.gap().getAdvertisingPayload();
jurica238814 3:b62a73dc76b7 221 adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA,
jurica238814 3:b62a73dc76b7 222 (uint8_t *)&advertisementPacket, sizeof(advertisementPacket));
jurica238814 0:442d98af8cc7 223 ble.gap().setAdvertisingPayload(adv_data);
Srishti12 4:cb3513aa9814 224 }