xl_ekf_context *xl_ekf_context_create() {
xl_ekf_context *ctx = (xl_ekf_context *) malloc(sizeof(xl_ekf_context));
memset(ctx, 0, sizeof(xl_ekf_context));
// ctx->sensor_manager = ASensorManager_getInstanceForPackage("com.cls.xl.xl");
ctx->sensor_manager = ASensorManager_getInstance();
ctx->acc = ASensorManager_getDefaultSensor(ctx->sensor_manager, ASENSOR_TYPE_ACCELEROMETER);
ctx->gyro = ASensorManager_getDefaultSensor(ctx->sensor_manager, ASENSOR_TYPE_GYROSCOPE);
ctx->acc_min_delay = ASensor_getMinDelay(ctx->acc);
ctx->gyro_min_delay = ASensor_getMinDelay(ctx->gyro);
if (ctx->acc == NULL || ctx->gyro == NULL) {
free(ctx);
return NULL;
}
ctx->looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
ctx->event_queue = ASensorManager_createEventQueue(
ctx->sensor_manager,
ctx->looper,
LOOPER_ID_USER,
NULL,
NULL
);
ASensorEventQueue_enableSensor(ctx->event_queue, ctx->acc);
ASensorEventQueue_enableSensor(ctx->event_queue, ctx->gyro);
int acc_delay = 20000;
int gyro_delay = 20000;
acc_delay = acc_delay > ctx->acc_min_delay ? acc_delay : ctx->acc_min_delay;
gyro_delay = gyro_delay > ctx->gyro_min_delay ? gyro_delay : ctx->gyro_min_delay;
ASensorEventQueue_setEventRate(ctx->event_queue, ctx->acc, acc_delay);
ASensorEventQueue_setEventRate(ctx->event_queue, ctx->gyro, gyro_delay);
ctx->lock = malloc(sizeof(pthread_mutex_t));
pthread_mutex_init(ctx->lock, NULL);
return ctx;
}
void setup_sensors() {
ASensorEvent event;
int events, ident;
ASensorManager * sensorManager;
const ASensor* accSensor;
const ASensor* gyroSensor;
const ASensor* magSensor;
void* sensor_data = malloc(10000);
LOGI("sensorValue() - ALooper_forThread()");
SENSORS_ENABLED = 1;
ALooper* looper = ALooper_forThread();
if (looper == NULL) {
looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
}
sensorManager = ASensorManager_getInstance();
accSensor = ASensorManager_getDefaultSensor(sensorManager,
ASENSOR_TYPE_ACCELEROMETER);
gyroSensor = ASensorManager_getDefaultSensor(sensorManager,
ASENSOR_TYPE_GYROSCOPE);
magSensor = ASensorManager_getDefaultSensor(sensorManager,
ASENSOR_TYPE_MAGNETIC_FIELD);
sensorEventQueue = ASensorManager_createEventQueue(sensorManager, looper, LOOPER_ID,
(ALooper_callbackFunc)get_sensorevents, sensor_data);
ASensorEventQueue_enableSensor(sensorEventQueue, accSensor);
ASensorEventQueue_enableSensor(sensorEventQueue, gyroSensor);
ASensorEventQueue_enableSensor(sensorEventQueue, magSensor);
//Sampling rate: 100Hz
int a = ASensor_getMinDelay(accSensor);
int b = ASensor_getMinDelay(gyroSensor);
int c = ASensor_getMinDelay(magSensor);
LOGI("min-delay: %d, %d, %d", a, b, c);
ASensorEventQueue_setEventRate(sensorEventQueue, accSensor, (1000L/SAMP_PER_SEC)*1000);
ASensorEventQueue_setEventRate(sensorEventQueue, gyroSensor, (1000L/SAMP_PER_SEC)*1000);
ASensorEventQueue_setEventRate(sensorEventQueue, magSensor, (1000L/SAMP_PER_SEC)*1000);
LOGI("sensorValue() - START");
while ((ident = ALooper_pollAll(-1, NULL, &events, NULL) >= 0)) {
// If a sensor has data, process it now.
if (ident == LOOPER_ID) {
ASensorEvent event;
while (ASensorEventQueue_getEvents(sensorEventQueue, &event, 1) > 0) {
LOGI("accelerometer X = %f y = %f z=%f ", event.acceleration.x, event.acceleration.y, event.acceleration.z);
}
}
}
}
status Sensor::enable()
{
if (mEventLoop.mEnabled)
{
mSensor = ASensorManager_getDefaultSensor(mEventLoop.mSensorManager, mSensorType);
if (mSensor != NULL)
{
if (ASensorEventQueue_enableSensor(mEventLoop.mSensorEventQueue, mSensor) < 0)
{
goto ERROR;
}
int32_t lMinDelay = ASensor_getMinDelay(mSensor);
if (ASensorEventQueue_setEventRate(mEventLoop.mSensorEventQueue, mSensor, lMinDelay) < 0)
{
goto ERROR;
}
} else
{
Pegas_log_error("No sensor type %d", mSensorType);
}
}
return STATUS_OK;
ERROR:
Pegas_log_error("Error while activating sensor.");
disable();
return STATUS_KO;
}
/*
* Class: io_quadroid_ndk_QuadroidLib
* Method: init
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_io_quadroid_UDPSensor_ndk_QuadroidLib_init(JNIEnv *env, jclass clazz) {
sensorManager = ASensorManager_getInstance();
looper = ALooper_forThread();
if(looper == NULL) {
looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
}
queue = ASensorManager_createEventQueue(sensorManager, looper, LOOPER_ID, get_sensor_events, NULL);
if(USE_PRESSURE==1) {
pressureSensor = ASensorManager_getDefaultSensor(sensorManager, 6);
ASensorEventQueue_enableSensor(queue, pressureSensor);
ASensorEventQueue_setEventRate(queue, pressureSensor, SAMP_PER_SEC);
}
if(USE_ACCELEROMETER==1) {
acceSensor = ASensorManager_getDefaultSensor(sensorManager, ASENSOR_TYPE_ACCELEROMETER);
ASensorEventQueue_enableSensor(queue, acceSensor);
ASensorEventQueue_setEventRate(queue, acceSensor, SAMP_PER_SEC);
log_acce = ASensor_getMinDelay(acceSensor);
}
if(USE_GYROSCOPE==1) {
gyroSensor = ASensorManager_getDefaultSensor(sensorManager, ASENSOR_TYPE_GYROSCOPE);
ASensorEventQueue_enableSensor(queue, gyroSensor);
ASensorEventQueue_setEventRate(queue, gyroSensor, SAMP_PER_SEC);
log_gyro = ASensor_getMinDelay(gyroSensor);
}
if(USE_MAGNETIC==1) {
magnSensor = ASensorManager_getDefaultSensor(sensorManager, ASENSOR_TYPE_MAGNETIC_FIELD);
ASensorEventQueue_enableSensor(queue, magnSensor);
ASensorEventQueue_setEventRate(queue, magnSensor, SAMP_PER_SEC);
log_magn = ASensor_getMinDelay(magnSensor);
}
if(USE_DISTANCE==1) {
proxSensor = ASensorManager_getDefaultSensor(sensorManager, ASENSOR_TYPE_PROXIMITY);
ASensorEventQueue_enableSensor(queue, proxSensor);
ASensorEventQueue_setEventRate(queue, proxSensor, SAMP_PER_SEC);
}
currentTime = time(0);
ALooper_pollAll(-1, NULL, &events, NULL);
}
JNIEXPORT void JNICALL Java_org_nzdis_sensorspeed_CMagneticField_magneticFieldStartup (JNIEnv *e, jclass c, jobject handler) {
updateHandler = handler;
env = e;
jclass handlerClass = env->FindClass("org/nzdis/sensorspeed/CMagneticField");
if (handlerClass == NULL) {
LOGI("big error 1");
}
mid = env->GetMethodID(handlerClass, "onSensorChanged", "()V");
if (mid == NULL) {
LOGI("big error 2");
}
ASensorEvent event;
int events, ident;
ASensorManager * sensorManager;
const ASensor* magSensor;
void* sensor_data = malloc(10000);
SENSORS_ENABLED = 1;
ALooper* looper = ALooper_forThread();
if (looper == NULL) {
looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
}
sensorManager = ASensorManager_getInstance();
magSensor = ASensorManager_getDefaultSensor(sensorManager, ASENSOR_TYPE_MAGNETIC_FIELD);
sensorEventQueue = ASensorManager_createEventQueue(sensorManager, looper, LOOPER_ID,
(ALooper_callbackFunc)get_sensorevents, sensor_data);
ASensorEventQueue_enableSensor(sensorEventQueue, magSensor);
int minDelay = ASensor_getMinDelay(magSensor);
//LOGI("min-delay: %d", minDelay);
ASensorEventQueue_setEventRate(sensorEventQueue, magSensor, (1000L/SAMP_PER_SEC)*1000);
while ((ident = ALooper_pollAll(-1, NULL, &events, NULL) >= 0)) {
// If a sensor has data, process it now.
if (ident == LOOPER_ID) {
LOGI("magneticFieldStartup() - LOOPER!!!!!!!!");
ASensorEvent event;
while (ASensorEventQueue_getEvents(sensorEventQueue, &event, 1) > 0) {
if (event.type == ASENSOR_TYPE_MAGNETIC_FIELD) {
env->CallVoidMethod(updateHandler, mid);
magneticfield_x = event.magnetic.x;
magneticfield_y = event.magnetic.y;
magneticfield_z = event.magnetic.z;
}
}
} else {
LOGI("magneticFieldStartup() - else!!!!!!!!!!!!!");
}
}
}
void SensorHandler::InitializeHandler() {
// Set and prepare looper
SensorHandler::tsInstance().looper = ALooper_forThread();
if (SensorHandler::tsInstance().looper == NULL) {
SensorHandler::tsInstance().looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
}
SensorHandler::tsInstance().sensorManager = ASensorManager_getInstance();
SensorHandler::tsInstance().accelerometerSensor = ASensorManager_getDefaultSensor(
SensorHandler::tsInstance().sensorManager, ASENSOR_TYPE_ACCELEROMETER);
SensorHandler::tsInstance().magneticSensor = ASensorManager_getDefaultSensor(
SensorHandler::tsInstance().sensorManager, ASENSOR_TYPE_MAGNETIC_FIELD);
SensorHandler::tsInstance().gyroscopeSensor = ASensorManager_getDefaultSensor(
SensorHandler::tsInstance().sensorManager, ASENSOR_TYPE_GYROSCOPE);
// Create event queue for sensor events
SensorHandler::tsInstance().sensorEventQueue = ASensorManager_createEventQueue(
SensorHandler::tsInstance().sensorManager, SensorHandler::tsInstance().looper,
3, SensorCallback, sensor_data);
ASensorEventQueue_enableSensor(SensorHandler::tsInstance().sensorEventQueue,
SensorHandler::tsInstance().accelerometerSensor);
ASensorEventQueue_enableSensor(SensorHandler::tsInstance().sensorEventQueue,
SensorHandler::tsInstance().magneticSensor);
ASensorEventQueue_enableSensor(SensorHandler::tsInstance().sensorEventQueue,
SensorHandler::tsInstance().gyroscopeSensor);
// Target sampling rate 100Hz
int a = ASensor_getMinDelay(accelerometerSensor);
int b = ASensor_getMinDelay(magneticSensor);
int c = ASensor_getMinDelay(gyroscopeSensor);
LOGI("Minimum delay; acceleration = %d magnetic = %d gyroscope = %d",a,b,c);
ASensorEventQueue_setEventRate(SensorHandler::tsInstance().sensorEventQueue,
SensorHandler::tsInstance().accelerometerSensor, 100000);
ASensorEventQueue_setEventRate(SensorHandler::tsInstance().sensorEventQueue,
SensorHandler::tsInstance().magneticSensor, 100000);
ASensorEventQueue_setEventRate(SensorHandler::tsInstance().sensorEventQueue,
SensorHandler::tsInstance().gyroscopeSensor, 100000);
}
bool SensorImpl::open(Sensor::Type sensor)
{
// Get the default sensor matching the type
m_sensor = getDefaultSensor(sensor);
// Sensor not available, stop here
if (!m_sensor)
return false;
// Get the minimum delay allowed between events
Time minimumDelay = microseconds(ASensor_getMinDelay(m_sensor));
// Set the event rate (not to consume too much battery)
ASensorEventQueue_setEventRate(sensorEventQueue, m_sensor, minimumDelay.asMicroseconds());
// Save the index of the sensor
m_index = static_cast<unsigned int>(sensor);
return true;
}
void QuerySensors() {
int I;
State->Manager = ASensorManager_getInstance();
State->Count = ASensorManager_getSensorList(State->Manager, &State->Sensors);
State->Info = (InfoStructure *) malloc(State->Count * sizeof(InfoStructure));
LOG(ANDROID_LOG_INFO, "Found %d sensors", State->Count);
for (I = 0; I < State->Count; I++) {
State->Info[I].Index = I;
State->Info[I].Type = ASensor_getType(State->Sensors[I]);
State->Info[I].Vendor = ASensor_getVendor(State->Sensors[I]);
State->Info[I].Name = ASensor_getName(State->Sensors[I]);
State->Info[I].Delay = ASensor_getMinDelay(State->Sensors[I]);
State->Info[I].Resolution = ASensor_getResolution(State->Sensors[I]);
LOG(ANDROID_LOG_INFO, "Sensor: %d, %s, %s, %dus, %f", State->Info[I].Type,
State->Info[I].Vendor,
State->Info[I].Name, State->Info[I].Delay,
State->Info[I].Resolution);
State->Info[I].Shift = 0;
}
}
void SensorImpl::setEnabled(const bool enabled)
{
if (enabled == isEnabled() || !isAvailable())
return;
if (enabled)
{
if (ASensorEventQueue_enableSensor(ns_sensorEventQueue, getDefault(m_type)) >= 0)
{
m_sensor = getDefault(m_type);
if (auto rate = ASensor_getMinDelay(m_sensor))
ASensorEventQueue_setEventRate(ns_sensorEventQueue, m_sensor, rate);
}
}
else
{
ASensorEventQueue_disableSensor(ns_sensorEventQueue, m_sensor);
m_sensor = NULL;
}
}
// try and open up the JoyWarrior file descriptor
bool CSensorAndroidBuiltIn::detect()
{
setType();
setPort();
// get the singleton instance of the m_pSensorManager
if (!m_pSensorManager) m_pSensorManager = (ASensorManager*) ASensorManager_getInstance();
/*
// sensor listing
ASensorList pSensorList = NULL;
int iNum = ASensorManager_getSensorList(m_pSensorManager, &pSensorList);
if (iNum && pSensorList) {
fprintf(stdout, "\n\n%d Detected Sensors:\n", iNum);
//int i = 0;
for (int i=0; i < iNum; i++) {
//while (i<10 && (pSensorList+i)) { //for (int i=0; i < iNum; i++) {
fprintf(stdout, " %s\n", ASensor_getName(*(pSensorList+i)));
i++;
}
}
else {
fprintf(stdout, "\n\nNo Sensor List? %d\n\n", iNum);
}
*/
// create looper
m_pLooper = ALooper_forThread(); // get existing looper
if (!m_pLooper) { // make new looper
//m_pLooper = ALooper_prepare(0);
m_pLooper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
}
if (!m_pLooper) { // no existing or new looper -- error
fprintf(stderr, "can't create Looper\n");
return false; // can't create looper
}
// setup event queue
//m_pSensorEventQueue = ASensorManager_createEventQueue(m_pSensorManager, m_pLooper,
// LOOPER_ID_QCN, QCN_ALooper_callback, &l_SensorVector);
m_pSensorEventQueue = ASensorManager_createEventQueue(m_pSensorManager, m_pLooper,
LOOPER_ID_QCN, NULL, NULL);
if (!m_pSensorEventQueue) {
fprintf(stderr, "can't create SensorEventQueue\n");
return false; // can't setup queue
}
// get the default accelerometer
m_pSensor = (ASensor*) ASensorManager_getDefaultSensor(m_pSensorManager, ASENSOR_TYPE_ACCELEROMETER);
if (!m_pSensor) {
//fprintf(stdout, "No Android accelerometer detected.\n");
return false; // no sensor
}
int iRetVal = 0;
if ((iRetVal = ASensorEventQueue_enableSensor(m_pSensorEventQueue, m_pSensor)) < 0) {
fprintf(stderr, "Error in enableSensor %d\n", iRetVal);
return false;
};
m_fResolution = ASensor_getResolution(m_pSensor);
m_minDelayMsec = ASensor_getMinDelay(m_pSensor);
int rateMsec = (int)((sm->dt > 0. ? sm->dt : g_DT) * 1000.);
//fprintf(stdout, "Rates: m_minDelayMSec = %d raceMsec = %d\n", m_minDelayMsec, rateMsec);
//if (rateMsec > m_minDelayMsec) m_minDelayMsec = rateMsec;
if (rateMsec < m_minDelayMsec) m_minDelayMsec = rateMsec;
fprintf(stdout, "Setting data rate to %d Hz\n", 1000L/m_minDelayMsec);
strlcpy(m_strSensor, ASensor_getName(m_pSensor), _MAX_PATH);
strlcpy(m_strVendor, ASensor_getVendor(m_pSensor), _MAX_PATH);
// NB: the rate is in microseconds!
if ((iRetVal = ASensorEventQueue_setEventRate(m_pSensorEventQueue, m_pSensor, m_minDelayMsec * 1000L)) < 0) {
fprintf(stderr, "Error in setEventRate %d\n", iRetVal);
// return false; // not really a fatal error
}
fprintf(stdout, "Android Default Sensor Detected: \n\n %s - %s\n"
" Res = %f --- Min Delay msec = %d\n"
" m_pSensor=%x m_pSensorEventQueue=%x\n",
m_strVendor, m_strSensor, m_fResolution, m_minDelayMsec, m_pSensor, m_pSensorEventQueue);
setType(SENSOR_ANDROID);
setSingleSampleDT(true); // set to true in raw mode so we don't get any interpolated/avg points (i.e. just the "integer" value hopefully)
return (bool)(getTypeEnum() == SENSOR_ANDROID);
}
