Degree Computation
Dependencies: aconno_SEGGER_RTT LSM9DS1 Si7006A20 adc52832_common aconnoMPL115A1 aconno_bsp
aconnoHelpers/aconnoHelpers.cpp
- Committer:
- Srishti12
- Date:
- 2019-01-08
- Revision:
- 4:cb3513aa9814
- Parent:
- 3:b62a73dc76b7
- Child:
- 5:6d20ad2a69ba
File content as of revision 4:cb3513aa9814:
/** * Made by Jurica @ aconno * jurica@aconno.de * More info @ aconno.de */ #include "mbed.h" #include "ble/BLE.h" #include "acd52832_bsp.h" #include "GapAdvertisingData.h" #include "Si7006A20.h" #include "LSM9DS1.h" #include "math.h" #include "adc52832_common/utilities.h" #include "MPL115A1.h" #include "acd_nrf52_saadc.h" #include "aconnoConfig.h" #include <iostream> #include <cmath> NRF52_SAADC analogIn; extern DigitalOut lightPower; extern DigitalOut temperaturePower; extern DigitalOut shdn; extern DigitalOut power; vector3_s memsAccelerometerInit; vector3_s memsGyroscopeInit; vector3_s memsMagnetometerInit; extern GapAdvertisingData adv_data; extern advertising_packet advertisementPacket; DigitalOut cs(p7); I2C i2c(I2C_DATA, I2C_CLK); Si7006 si(&i2c); SPI spi(SPI_MOSI, SPI_MISO, SPI_SCLK); MPL115A1 mpl115a1(spi, cs); LSM9DS1 mems(&i2c); float getLight(){ return ((float)analogIn.getData()[1])/ADC_RESOLUTION * VALUE_TO_PERCENTAGE; } float voltage2temp(float vOut){ return ((float)vOut - (float)V0)/((float)TC); } float getTemperature(){ return voltage2temp(((float)analogIn.getData()[2])/ADC_RESOLUTION * (float)VCC); } uint8_t getBattery(){ uint16_t batteryVoltage = analogIn.getData()[0]; const uint16_t zero_percent_limit = 739; const uint16_t onehundred_percent_limit = 810; const uint16_t percentage_increments = 5; uint8_t percentage; if (batteryVoltage < zero_percent_limit) { percentage = 0; } else if(batteryVoltage > onehundred_percent_limit) { percentage = 100; } else { batteryVoltage -= zero_percent_limit; percentage = (batteryVoltage*100)/(onehundred_percent_limit - zero_percent_limit); percentage = percentage/percentage_increments*percentage_increments; } return percentage; } float getHumidity(){ float result; si.getHumidity(&result); return result; } void readGyroscope(vector3_s *gyroscopeData){ mems.readGyroscope((int16_t *)gyroscopeData); *gyroscopeData -= memsGyroscopeInit; } void readAccelerometer(vector3_s *accelerometerData){ mems.readAccelerometer((int16_t *)accelerometerData); *accelerometerData -= memsAccelerometerInit; } void readMagnetometer(vector3_s *magnetometerData){ mems.readMagnetometer((int16_t *)magnetometerData); *magnetometerData -= memsMagnetometerInit; } void calibrateAccelerometer(){ vector3_s accelerometerData; for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){ readAccelerometer(&accelerometerData); memsAccelerometerInit += accelerometerData; } memsAccelerometerInit /= CALIBRATION_STEPS; } void calibrateGyroscope(){ vector3_s gyroscopeData; for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){ readGyroscope(&gyroscopeData); memsGyroscopeInit += gyroscopeData; } memsGyroscopeInit /= CALIBRATION_STEPS; } void calibrateMag(){ vector3_s magnetometerData; for(uint8_t counter = 0; counter < CALIBRATION_STEPS; ++counter){ readMagnetometer(&magnetometerData); memsMagnetometerInit += magnetometerData; } memsMagnetometerInit /= CALIBRATION_STEPS; } float calculateDegree(float accX, float accY, float accZ){ float degree; if (accX != 0.0f && accY != 0.0f && accZ != 0.0f) { float dnum = sqrt(accY * accY + accZ * accZ); degree = atan(accX / dnum); if ((accX < 0 && accY >= 0)) { degree = 360 + degree; } if (accX >= 0 && accY < 0) degree = 180 - degree; if (accX < 0 && accY < 0) degree = 180 - degree; //cout<<"degree: " << degree; } return degree; } void updateData() { //static uint8_t advertisementType = 0; int16_t temp_acc[3]; BLE &ble = BLE::Instance(); float result; mems.startAccelerometer(); // advertisementPacket.type = 0x00; //Added for Angular sensa readAccelerometer((vector3_s *)temp_acc); float degreeValue = calculateDegree( (float) temp_acc[1], (float) temp_acc[0], (float) temp_acc[2]); cout<<"degree: " << degreeValue; advertisementPacket.degree = degreeValue; /*if(advertisementType < 1) { power = 1; mems.startAccelerometer(); mems.startGyroscope(); mems.startMagnetometer(); wait_ms(WAKEUP_TIME_DELAY_MS); advertisementPacket.type = 0x00; readGyroscope((vector3_s *)advertisementPacket.gyroscope); readAccelerometer((vector3_s *)temp_acc); readMagnetometer((vector3_s *)advertisementPacket.magnetometer); //advertisementPacket.acc_lsb_value = (0xF9E);//(0x3D80); //advertisementPacket.accelerometer[0] = temp_acc[1]; //advertisementPacket.accelerometer[1] = temp_acc[0]; //advertisementPacket.accelerometer[2] = temp_acc[2]; float degreeValue = calculateDegree( (float) temp_acc[1], (float) temp_acc[0], (float) temp_acc[2]); cout<<"degree: " << degreeValue; advertisementPacket.degree = degreeValue; // ^--- That's in ug cuz MSB is 1 } else { NRF_SAADC->ENABLE = 1; power = 1; temperaturePower = 1; lightPower = 1; shdn = 1; analogIn.addChannel(9); // Set VDD as source to SAADC analogIn.addChannel(6); // Light analogIn.addChannel(7); // Temp analogIn.calibrate(); wait_ms(WAKEUP_TIME_DELAY_MS); analogIn.updateData(); wait_ms(WAKEUP_TIME_DELAY_MS); advertisementPacket.type = 0x01; advertisementPacket.temperature = getTemperature(); advertisementPacket.light = getLight(); advertisementPacket.humidity = getHumidity(); advertisementPacket.pressure = mpl115a1.getPressure(); advertisementPacket.battery = getBattery(); } */ SPI dummySpi(SPI_MOSI, SPI_MISO, SPI_SCLK); power = 0; shdn = 0; temperaturePower = 0; lightPower = 0; NRF_SAADC->ENABLE = 0; //if(++advertisementType > 2) advertisementType = 0; adv_data = ble.gap().getAdvertisingPayload(); adv_data.updateData(adv_data.MANUFACTURER_SPECIFIC_DATA, (uint8_t *)&advertisementPacket, sizeof(advertisementPacket)); ble.gap().setAdvertisingPayload(adv_data); }