Merge pull request #810 from AuroraRAS/androidsensor

add androidsensor rotator
pull/816/head
Michael Black 2021-09-24 22:24:15 -05:00 zatwierdzone przez GitHub
commit 1f9ffe3035
Nie znaleziono w bazie danych klucza dla tego podpisu
ID klucza GPG: 4AEE18F83AFDEB23
11 zmienionych plików z 476 dodań i 2 usunięć

Wyświetl plik

@ -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

Wyświetl plik

@ -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.
*

Wyświetl plik

@ -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)

Wyświetl plik

@ -0,0 +1,137 @@
/*
* Hamlib Rotator backend - Radant
* Copyright (c) 2001-2003 by Stephane Fillod
* Contributed by Francois Retief <fgretief@sun.ac.za>
* Copyright (c) 2014 by Alexander Schultze <alexschultze@gmail.com>
*
*
* 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 <stdio.h>
#include <stdlib.h>
#include <string.h> /* String function definitions */
#include <unistd.h> /* UNIX standard function definitions */
#include <math.h>
#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 */

Wyświetl plik

@ -0,0 +1,29 @@
/*
* Hamlib Rotator backend - Easycomm interface protocol
* Copyright (c) 2001-2003 by Stephane Fillod
* Contributed by Francois Retief <fgretief@sun.ac.za>
* Copyright (c) 2014 by Alexander Schultze <alexschultze@gmail.com>
*
*
* 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 */

Wyświetl plik

@ -0,0 +1,207 @@
#include "ndkimu.h"
#include <cmath>
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;
}

Wyświetl plik

@ -0,0 +1,57 @@
#ifndef NDKIMU_H
#define NDKIMU_H
#include <dlfcn.h>
#include <GLES2/gl2.h>
#include <android/looper.h>
#include <android/sensor.h>
#include <cstdint>
#include <cassert>
#include <string>
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

Wyświetl plik

@ -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)

Wyświetl plik

@ -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 */
};

12
tests/Android.mk 100644
Wyświetl plik

@ -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)

Wyświetl plik

@ -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