diff --git a/Android.mk b/Android.mk index e59ea0b87..fd6cb4dc3 100644 --- a/Android.mk +++ b/Android.mk @@ -49,3 +49,6 @@ include $(TOP_PATH)/rotators/satel/Android.mk include $(TOP_PATH)/rotators/spid/Android.mk include $(TOP_PATH)/rotators/ts7400/Android.mk include $(TOP_PATH)/rotators/radant/Android.mk +include $(TOP_PATH)/rotators/androidsensor/Android.mk + +include $(TOP_PATH)/tests/Android.mk diff --git a/include/hamlib/rotlist.h b/include/hamlib/rotlist.h index 939cad56b..522d4acbb 100644 --- a/include/hamlib/rotlist.h +++ b/include/hamlib/rotlist.h @@ -619,6 +619,11 @@ #define ROT_MODEL_RADANT ROT_MAKE_MODEL(ROT_RADANT, 1) +#define ROT_ANDROIDSENSOR 23 +#define ROT_BACKEND_ANDROIDSENSOR "androidsensor" +#define ROT_MODEL_ANDROIDSENSOR ROT_MAKE_MODEL(ROT_ANDROIDSENSOR, 1) + + /** * \brief Convenience type definition for a rotator model. * diff --git a/rotators/androidsensor/Android.mk b/rotators/androidsensor/Android.mk new file mode 100644 index 000000000..8c0ab8151 --- /dev/null +++ b/rotators/androidsensor/Android.mk @@ -0,0 +1,12 @@ +LOCAL_PATH:= $(call my-dir) + +include $(CLEAR_VARS) + +LOCAL_SRC_FILES := androidsensor.cpp ndkimu.cpp +LOCAL_MODULE := androidsensor + +LOCAL_CFLAGS := -DHAVE_CONFIG_H +LOCAL_C_INCLUDES := android include src +LOCAL_LDLIBS := -lhamlib -Lobj/local/$(TARGET_ARCH_ABI) + +include $(BUILD_STATIC_LIBRARY) diff --git a/rotators/androidsensor/androidsensor.cpp b/rotators/androidsensor/androidsensor.cpp new file mode 100644 index 000000000..e53c12576 --- /dev/null +++ b/rotators/androidsensor/androidsensor.cpp @@ -0,0 +1,137 @@ +/* + * Hamlib Rotator backend - Radant + * Copyright (c) 2001-2003 by Stephane Fillod + * Contributed by Francois Retief + * Copyright (c) 2014 by Alexander Schultze + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include /* String function definitions */ +#include /* UNIX standard function definitions */ +#include + +#include "hamlib/rotator.h" +#include "register.h" + +#include "androidsensor.h" +#include "ndkimu.cpp" + +/* ************************************************************************* */ + +static NdkImu *imu; + +static int +androidsensor_rot_set_position(ROT *rot, azimuth_t az, elevation_t el) +{ + rig_debug(RIG_DEBUG_TRACE, "%s called: %f %f\n", __func__, az, el); + // To-Do + return RIG_OK; +} + +static int +androidsensor_rot_get_position(ROT *rot, azimuth_t *az, elevation_t *el) +{ + rig_debug(RIG_DEBUG_TRACE, "%s called: ", __func__); + + imu->update(); + + float accData[3], magData[3]; + imu->getAccData(accData); + imu->getMagData(magData); + + float R[9] = {0}, I[9] = {0}, orientation[3] = {0}; + bool status = imu->getRotationMatrix(R, 9, I, 9, accData, magData); + if(status) + imu->getOrientation(R, 9, orientation); + + *az = (orientation[0] / M_PI * 180 ); + *el = (orientation[1] / M_PI * 180 ) * -1; + + rig_debug(RIG_DEBUG_TRACE, "%f %f\n", az, el); + + return RIG_OK; +} + +static int +androidsensor_rot_stop(ROT *rot) +{ + rig_debug(RIG_DEBUG_TRACE, "%s called\n", __func__); + // To-Do + return RIG_OK; +} + +static int +androidsensor_rot_init(ROT *rot) +{ + rig_debug(RIG_DEBUG_TRACE, "%s called, make new NdkImu\n", __func__); + imu = new NdkImu(); + return RIG_OK; +} + +static int +androidsensor_rot_cleanup(ROT *rot) +{ + rig_debug(RIG_DEBUG_TRACE, "%s called, delete imu\n", __func__); + delete imu; + return RIG_OK; +} + +/* ************************************************************************* */ + +const struct rot_caps androidsensor_rot_caps = +{ + ROT_MODEL(ROT_MODEL_ANDROIDSENSOR), + .model_name = "ACC/MAG", + .mfg_name = "Android Sensor", + .version = "20210925.0", + .copyright = "LGPL", + .rot_type = ROT_TYPE_AZEL, + .port_type = RIG_PORT_NONE, + + .min_az = -180.0, + .max_az = 180.0, + .min_el = -180.0, + .max_el = 180.0, + + .priv = NULL, /* priv */ + + .set_position = androidsensor_rot_set_position, + .get_position = androidsensor_rot_get_position, + .stop = androidsensor_rot_stop, + .rot_init = androidsensor_rot_init, + .rot_cleanup = androidsensor_rot_cleanup, +}; + +/* ************************************************************************* */ + +DECLARE_INITROT_BACKEND(androidsensor) +{ + rig_debug(RIG_DEBUG_VERBOSE, "%s called\n", __func__); + + rot_register(&androidsensor_rot_caps); + return RIG_OK; +} + +/* ************************************************************************* */ +/* end of file */ diff --git a/rotators/androidsensor/androidsensor.h b/rotators/androidsensor/androidsensor.h new file mode 100644 index 000000000..89878a5d3 --- /dev/null +++ b/rotators/androidsensor/androidsensor.h @@ -0,0 +1,29 @@ +/* + * Hamlib Rotator backend - Easycomm interface protocol + * Copyright (c) 2001-2003 by Stephane Fillod + * Contributed by Francois Retief + * Copyright (c) 2014 by Alexander Schultze + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifndef _ROT_ANDROIDSENSOR_H +#define _ROT_ANDROIDSENSOR_H 1 + +extern const struct rot_caps androidsensor_rot_caps; + +#endif /* _ROT_ANDROIDSENSOR_H */ diff --git a/rotators/androidsensor/ndkimu.cpp b/rotators/androidsensor/ndkimu.cpp new file mode 100644 index 000000000..36d8ea511 --- /dev/null +++ b/rotators/androidsensor/ndkimu.cpp @@ -0,0 +1,207 @@ +#include "ndkimu.h" + +#include + +NdkImu::NdkImu(char kPackageName[]) +{ + 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[]) +{ + 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); + } + + 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, float gravity[], 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(float R[], size_t Rlength, float valuesBuf[]) +{ + if(valuesBuf == NULL) + 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) + 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) + magDataBuf = (float*)malloc(sizeof (float)*3); + magDataBuf[0] = sensorMagDataFilter.x; + magDataBuf[1] = sensorMagDataFilter.y; + magDataBuf[2] = sensorMagDataFilter.z; + return magDataBuf; +} diff --git a/rotators/androidsensor/ndkimu.h b/rotators/androidsensor/ndkimu.h new file mode 100644 index 000000000..9225d9bf3 --- /dev/null +++ b/rotators/androidsensor/ndkimu.h @@ -0,0 +1,57 @@ +#ifndef NDKIMU_H +#define NDKIMU_H + +#include +#include + +#include +#include + +#include +#include +#include + + +static const int LOOPER_ID_USER = 3; +static const int SENSOR_HISTORY_LENGTH = 100; +static const int SENSOR_REFRESH_RATE_HZ = 100; +static constexpr int32_t SENSOR_REFRESH_PERIOD_US = int32_t(1000000 / SENSOR_REFRESH_RATE_HZ); +static const float SENSOR_FILTER_ALPHA = 0.1f; + +class NdkImu +{ +public: + NdkImu(char kPackageName[] = NULL); + ~NdkImu(); + static ASensorManager *AcquireASensorManagerInstance(char kPackageName[]); + static bool getRotationMatrix(float R[], size_t Rlength, float I[], size_t Ilength, float gravity[], float geomagnetic[]); + static float *getOrientation(float R[], size_t Rlength, float valuesBuf[] = NULL); + +private: + ASensorManager *sensorManager; + const ASensor *accelerometer; + ASensorEventQueue *accelerometerEventQueue; + const ASensor *geomagnetic; + ASensorEventQueue *geomagneticEventQueue; + ALooper *looper; + + struct SensorData { + GLfloat x; + GLfloat y; + GLfloat z; + }; + SensorData sensorAccData[SENSOR_HISTORY_LENGTH*2]; + SensorData sensorAccDataFilter; + SensorData sensorMagData[SENSOR_HISTORY_LENGTH*2]; + SensorData sensorMagDataFilter; + + int sensorAccDataIndex; + int sensorMagDataIndex; + +public: + void update(); + float *getAccData(float accDataBuf[] = NULL); + float *getMagData(float magDataBuf[] = NULL); +}; + +#endif // NDKIMU_H diff --git a/src/Android.mk b/src/Android.mk index d4a1f3158..24090e7c3 100644 --- a/src/Android.mk +++ b/src/Android.mk @@ -39,8 +39,8 @@ LOCAL_STATIC_LIBRARIES := adat alinco amsat aor ars barrett celestron cnctrk \ gs232a heathkit icmarine icom ioptron jrc kachina kenwood kit \ lowe m2 meade pcr prm80 prosistel racal rft \ rotorez rs sartek satel skanti spid tapr tentec ts7400 tuner \ - uniden wj yaesu radant + uniden wj yaesu radant androidsensor -LOCAL_LDLIBS := -llog +LOCAL_LDLIBS := -llog -landroid include $(BUILD_SHARED_LIBRARY) diff --git a/src/rot_reg.c b/src/rot_reg.c index 3a97ed71d..05494a9c6 100644 --- a/src/rot_reg.c +++ b/src/rot_reg.c @@ -89,6 +89,9 @@ DEFINE_INITROT_BACKEND(radant); #if HAVE_LIBINDI DEFINE_INITROT_BACKEND(indi); #endif +#if defined(ANDROID) || defined(__ANDROID__) +DEFINE_INITROT_BACKEND(androidsensor); +#endif //! @endcond /** @@ -132,6 +135,9 @@ static struct { ROT_RADANT, ROT_BACKEND_RADANT, ROT_FUNCNAMA(radant)}, #if HAVE_LIBINDI { ROT_INDI, ROT_BACKEND_INDI, ROT_FUNCNAMA(indi) }, +#endif +#if defined(ANDROID) || defined(__ANDROID__) + { ROT_ANDROIDSENSOR, ROT_BACKEND_ANDROIDSENSOR, ROT_FUNCNAMA(androidsensor) }, #endif { 0, NULL }, /* end */ }; diff --git a/tests/Android.mk b/tests/Android.mk new file mode 100644 index 000000000..785b6d6f8 --- /dev/null +++ b/tests/Android.mk @@ -0,0 +1,12 @@ +LOCAL_PATH:= $(call my-dir) + +include $(CLEAR_VARS) + +LOCAL_SRC_FILES := rotctld.c rotctl_parse.c dumpcaps_rot.c ../src/rot_settings.c +LOCAL_MODULE := rotctld + +LOCAL_CFLAGS := -DHAVE_CONFIG_H +LOCAL_C_INCLUDES := android include src +LOCAL_LDLIBS := -lhamlib -Lobj/local/$(TARGET_ARCH_ABI) + +include $(BUILD_EXECUTABLE) diff --git a/tests/rotctld.c b/tests/rotctld.c index b714265bb..19b12ce3b 100644 --- a/tests/rotctld.c +++ b/tests/rotctld.c @@ -664,6 +664,9 @@ void *handle_socket(void *arg) } fsockin = _fdopen(sock_osfhandle, "rb"); +#elif defined(ANDROID) || defined(__ANDROID__) + // fdsan does not allow fdopen the same fd twice in Android + fsockin = fdopen(dup(handle_data_arg->sock), "rb"); #else fsockin = fdopen(handle_data_arg->sock, "rb"); #endif @@ -676,6 +679,9 @@ void *handle_socket(void *arg) #ifdef __MINGW32__ fsockout = _fdopen(sock_osfhandle, "wb"); +#elif defined(ANDROID) || defined(__ANDROID__) + // fdsan does not allow fdopen the same fd twice in Android + fsockout = fdopen(dup(handle_data_arg->sock), "wb"); #else fsockout = fdopen(handle_data_arg->sock, "wb"); #endif