#include "ndkimu.h" #include NdkImu::NdkImu(char kPackageName[]) { sensorAccDataIndex = 0; sensorMagDataIndex = 0; sensorManager = AcquireASensorManagerInstance(kPackageName); assert(sensorManager != NULL); accelerometer = ASensorManager_getDefaultSensor(sensorManager, ASENSOR_TYPE_ACCELEROMETER); geomagnetic = ASensorManager_getDefaultSensor(sensorManager, ASENSOR_TYPE_MAGNETIC_FIELD); assert(accelerometer != NULL); looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS); assert(looper != NULL); accelerometerEventQueue = ASensorManager_createEventQueue(sensorManager, looper, LOOPER_ID_USER, NULL, NULL); geomagneticEventQueue = ASensorManager_createEventQueue(sensorManager, looper, LOOPER_ID_USER, NULL, NULL); assert(accelerometerEventQueue != NULL); auto status = ASensorEventQueue_enableSensor(accelerometerEventQueue, accelerometer); assert(status >= 0); status = ASensorEventQueue_setEventRate(accelerometerEventQueue, accelerometer, SENSOR_REFRESH_PERIOD_US); assert(status >= 0); status = ASensorEventQueue_enableSensor(geomagneticEventQueue, geomagnetic); assert(status >= 0); status = ASensorEventQueue_setEventRate(geomagneticEventQueue, geomagnetic, SENSOR_REFRESH_PERIOD_US); assert(status >= 0); } NdkImu::~NdkImu() { auto status = ASensorEventQueue_disableSensor(accelerometerEventQueue, accelerometer); assert(status >= 0); status = ASensorEventQueue_disableSensor(geomagneticEventQueue, geomagnetic); assert(status >= 0); } ASensorManager *NdkImu::AcquireASensorManagerInstance(char kPackageName[]) { // cppcheck-suppress cstyleCast typedef ASensorManager *(*PF_GETINSTANCEFORPACKAGE)(const char *name); void* androidHandle = dlopen("libandroid.so", RTLD_NOW); PF_GETINSTANCEFORPACKAGE getInstanceForPackageFunc = (PF_GETINSTANCEFORPACKAGE) dlsym(androidHandle, "ASensorManager_getInstanceForPackage"); if (getInstanceForPackageFunc) { return getInstanceForPackageFunc(kPackageName); } // cppcheck-suppress cstyleCast typedef ASensorManager *(*PF_GETINSTANCE)(); PF_GETINSTANCE getInstanceFunc = (PF_GETINSTANCE) dlsym(androidHandle, "ASensorManager_getInstance"); // by all means at this point, ASensorManager_getInstance should be available assert(getInstanceFunc); return getInstanceFunc(); } /* * https://android.googlesource.com/platform/frameworks/base/+/master/core/java/android/hardware/SensorManager.java * blob: 9e78a6bb476061b4e20378ba20a00a2761e1ed7e * line 1174 */ bool NdkImu::getRotationMatrix(float R[], size_t Rlength, float I[], size_t Ilength, const float gravity[], const float geomagnetic[]) { // TODO: move this to native code for efficiency float Ax = gravity[0]; float Ay = gravity[1]; float Az = gravity[2]; const float normsqA = (Ax * Ax + Ay * Ay + Az * Az); const float g = 9.81f; const float freeFallGravitySquared = 0.01f * g * g; if (normsqA < freeFallGravitySquared) { // gravity less than 10% of normal value return false; } const float Ex = geomagnetic[0]; const float Ey = geomagnetic[1]; const float Ez = geomagnetic[2]; float Hx = Ey * Az - Ez * Ay; float Hy = Ez * Ax - Ex * Az; float Hz = Ex * Ay - Ey * Ax; const float normH = (float) std::sqrt(Hx * Hx + Hy * Hy + Hz * Hz); if (normH < 0.1f) { // device is close to free fall (or in space?), or close to // magnetic north pole. Typical values are > 100. return false; } const float invH = 1.0f / normH; Hx *= invH; Hy *= invH; Hz *= invH; const float invA = 1.0f / (float) std::sqrt(Ax * Ax + Ay * Ay + Az * Az); Ax *= invA; Ay *= invA; Az *= invA; const float Mx = Ay * Hz - Az * Hy; const float My = Az * Hx - Ax * Hz; const float Mz = Ax * Hy - Ay * Hx; if (R != NULL) { if (Rlength == 9) { R[0] = Hx; R[1] = Hy; R[2] = Hz; R[3] = Mx; R[4] = My; R[5] = Mz; R[6] = Ax; R[7] = Ay; R[8] = Az; } else if (Rlength == 16) { R[0] = Hx; R[1] = Hy; R[2] = Hz; R[3] = 0; R[4] = Mx; R[5] = My; R[6] = Mz; R[7] = 0; R[8] = Ax; R[9] = Ay; R[10] = Az; R[11] = 0; R[12] = 0; R[13] = 0; R[14] = 0; R[15] = 1; } } if (I != NULL) { // compute the inclination matrix by projecting the geomagnetic // vector onto the Z (gravity) and X (horizontal component // of geomagnetic vector) axes. const float invE = 1.0f / (float) std::sqrt(Ex * Ex + Ey * Ey + Ez * Ez); const float c = (Ex * Mx + Ey * My + Ez * Mz) * invE; const float s = (Ex * Ax + Ey * Ay + Ez * Az) * invE; if (Ilength == 9) { I[0] = 1; I[1] = 0; I[2] = 0; I[3] = 0; I[4] = c; I[5] = s; I[6] = 0; I[7] = -s; I[8] = c; } else if (Ilength == 16) { I[0] = 1; I[1] = 0; I[2] = 0; I[4] = 0; I[5] = c; I[6] = s; I[8] = 0; I[9] = -s; I[10] = c; I[3] = I[7] = I[11] = I[12] = I[13] = I[14] = 0; I[15] = 1; } } return true; } /* * https://android.googlesource.com/platform/frameworks/base/+/master/core/java/android/hardware/SensorManager.java * blob: 9e78a6bb476061b4e20378ba20a00a2761e1ed7e * line 1469 */ float *NdkImu::getOrientation(const float R[], size_t Rlength, float valuesBuf[]) { if(valuesBuf == NULL) { // cppcheck-suppress cstyleCast valuesBuf = (float*)malloc(sizeof (float)*3); } if (Rlength == 9) { valuesBuf[0] = (float) std::atan2(R[1], R[4]); valuesBuf[1] = (float) std::asin(-R[7]); valuesBuf[2] = (float) std::atan2(-R[6], R[8]); } else { valuesBuf[0] = (float) std::atan2(R[1], R[5]); valuesBuf[1] = (float) std::asin(-R[9]); valuesBuf[2] = (float) std::atan2(-R[8], R[10]); } return valuesBuf; } void NdkImu::update() { ALooper_pollAll(0, NULL, NULL, NULL); ASensorEvent event; const float a = SENSOR_FILTER_ALPHA; while (ASensorEventQueue_getEvents(accelerometerEventQueue, &event, 1) > 0) { sensorAccDataFilter.x = a * event.acceleration.x + (1.0f - a) * sensorAccDataFilter.x; sensorAccDataFilter.y = a * event.acceleration.y + (1.0f - a) * sensorAccDataFilter.y; sensorAccDataFilter.z = a * event.acceleration.z + (1.0f - a) * sensorAccDataFilter.z; } sensorAccData[sensorAccDataIndex] = sensorAccDataFilter; sensorAccData[SENSOR_HISTORY_LENGTH+sensorAccDataIndex] = sensorAccDataFilter; sensorAccDataIndex = (sensorAccDataIndex + 1) % SENSOR_HISTORY_LENGTH; while (ASensorEventQueue_getEvents(geomagneticEventQueue, &event, 1) > 0) { sensorMagDataFilter.x = a * event.magnetic.x + (1.0f - a) * sensorMagDataFilter.x; sensorMagDataFilter.y = a * event.magnetic.y + (1.0f - a) * sensorMagDataFilter.y; sensorMagDataFilter.z = a * event.magnetic.z + (1.0f - a) * sensorMagDataFilter.z; } sensorMagData[sensorMagDataIndex] = sensorMagDataFilter; sensorMagData[SENSOR_HISTORY_LENGTH+sensorMagDataIndex] = sensorMagDataFilter; sensorMagDataIndex = (sensorMagDataIndex + 1) % SENSOR_HISTORY_LENGTH; } float *NdkImu::getAccData(float accDataBuf[]) { if(accDataBuf == NULL) { // cppcheck-suppress cstyleCast accDataBuf = (float*)malloc(sizeof (float)*3); } accDataBuf[0] = sensorAccDataFilter.x; accDataBuf[1] = sensorAccDataFilter.y; accDataBuf[2] = sensorAccDataFilter.z; return accDataBuf; } float *NdkImu::getMagData(float magDataBuf[]) { if(magDataBuf == NULL) { // cppcheck-suppress cstyleCast magDataBuf = (float*)malloc(sizeof (float)*3); } magDataBuf[0] = sensorMagDataFilter.x; magDataBuf[1] = sensorMagDataFilter.y; magDataBuf[2] = sensorMagDataFilter.z; return magDataBuf; }