Degree Computation
Dependencies: aconno_SEGGER_RTT LSM9DS1 Si7006A20 adc52832_common aconnoMPL115A1 aconno_bsp
Diff: aconnoHelpers/aconnoHelpers.cpp
- Revision:
- 4:cb3513aa9814
- Parent:
- 3:b62a73dc76b7
- Child:
- 5:6d20ad2a69ba
--- a/aconnoHelpers/aconnoHelpers.cpp Fri Dec 14 13:33:03 2018 +0100 +++ b/aconnoHelpers/aconnoHelpers.cpp Tue Jan 08 13:06:07 2019 +0000 @@ -15,6 +15,8 @@ #include "MPL115A1.h" #include "acd_nrf52_saadc.h" #include "aconnoConfig.h" +#include <iostream> +#include <cmath> NRF52_SAADC analogIn; extern DigitalOut lightPower; @@ -120,15 +122,48 @@ 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; + //static uint8_t advertisementType = 0; int16_t temp_acc[3]; BLE &ble = BLE::Instance(); float result; - - if(advertisementType < 1) + 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(); @@ -139,10 +174,17 @@ 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]; + //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 @@ -167,7 +209,7 @@ advertisementPacket.pressure = mpl115a1.getPressure(); advertisementPacket.battery = getBattery(); } - + */ SPI dummySpi(SPI_MOSI, SPI_MISO, SPI_SCLK); power = 0; shdn = 0; @@ -175,9 +217,9 @@ lightPower = 0; NRF_SAADC->ENABLE = 0; - if(++advertisementType > 2) advertisementType = 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); -} \ No newline at end of file +}