186 lines
6.8 KiB
C++
186 lines
6.8 KiB
C++
/*
|
|
* STMicroelectronics Accelerometer Sensor Class
|
|
*
|
|
* Copyright 2015-2016 STMicroelectronics Inc.
|
|
* Author: Denis Ciocca - <denis.ciocca@st.com>
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License").
|
|
*/
|
|
|
|
#include <fcntl.h>
|
|
#include <assert.h>
|
|
#include <signal.h>
|
|
|
|
#include "Accelerometer.h"
|
|
|
|
#ifdef CONFIG_ST_HAL_ACCEL_CALIB_ENABLED
|
|
#define CALIBRATION_FREQUENCY 25
|
|
#define CALIBRATION_PERIOD_MS (1000.0f / CALIBRATION_FREQUENCY)
|
|
|
|
extern "C" {
|
|
#include "STAccCalibration_API.h"
|
|
}
|
|
#endif /* CONFIG_ST_HAL_ACCEL_CALIB_ENABLED */
|
|
|
|
Accelerometer::Accelerometer(HWSensorBaseCommonData *data, const char *name,
|
|
struct device_iio_sampling_freqs *sfa, int handle,
|
|
unsigned int hw_fifo_len, float power_consumption, bool wakeup) :
|
|
HWSensorBaseWithPollrate(data, name, sfa, handle,
|
|
SENSOR_TYPE_ACCELEROMETER, hw_fifo_len, power_consumption)
|
|
{
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION > ST_HAL_KITKAT_VERSION)
|
|
sensor_t_data.stringType = SENSOR_STRING_TYPE_ACCELEROMETER;
|
|
#ifdef CONFIG_ST_HAL_DIRECT_REPORT_SENSOR
|
|
/* Support Direct Channel in Android O */
|
|
sensor_t_data.flags |= (SENSOR_FLAG_CONTINUOUS_MODE |
|
|
SENSOR_FLAG_DIRECT_CHANNEL_ASHMEM |
|
|
(SENSOR_DIRECT_RATE_VERY_FAST <<
|
|
SENSOR_FLAG_SHIFT_DIRECT_REPORT));
|
|
#else /* CONFIG_ST_HAL_DIRECT_REPORT_SENSOR */
|
|
sensor_t_data.flags |= SENSOR_FLAG_CONTINUOUS_MODE;
|
|
#endif /* CONFIG_ST_HAL_DIRECT_REPORT_SENSOR */
|
|
|
|
if (wakeup)
|
|
sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP;
|
|
#else /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
(void)wakeup;
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
|
|
sensor_t_data.resolution = data->channels[0].scale;
|
|
sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2, data->channels[0].bits_used - 1) - 1);
|
|
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_PIE_VERSION)
|
|
#if (CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED)
|
|
supportsSensorAdditionalInfo = true;
|
|
#endif /* CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED */
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|
|
}
|
|
|
|
Accelerometer::~Accelerometer()
|
|
{
|
|
|
|
}
|
|
|
|
int Accelerometer::Enable(int handle, bool enable, bool lock_en_mutex)
|
|
{
|
|
#ifdef CONFIG_ST_HAL_ACCEL_CALIB_ENABLED
|
|
int err;
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_lock(&enable_mutex);
|
|
|
|
err = HWSensorBaseWithPollrate::Enable(handle, enable, false);
|
|
if (err < 0) {
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return err;
|
|
}
|
|
|
|
if (enable)
|
|
ST_AccCalibration_API_Init(CALIBRATION_PERIOD_MS);
|
|
else
|
|
ST_AccCalibration_API_DeInit(CALIBRATION_PERIOD_MS);
|
|
|
|
if (lock_en_mutex)
|
|
pthread_mutex_unlock(&enable_mutex);
|
|
|
|
return 0;
|
|
#else /* CONFIG_ST_HAL_ACCEL_CALIB_ENABLED */
|
|
return HWSensorBaseWithPollrate::Enable(handle, enable, lock_en_mutex);
|
|
#endif /* CONFIG_ST_HAL_ACCEL_CALIB_ENABLED */
|
|
}
|
|
|
|
void Accelerometer::ProcessData(SensorBaseData *data)
|
|
{
|
|
float tmp_raw_data[SENSOR_DATA_3AXIS];
|
|
#ifdef CONFIG_ST_HAL_ACCEL_CALIB_ENABLED
|
|
STAccCalibration_Input acc_cal_input;
|
|
STAccCalibration_Output acc_cal_output;
|
|
#endif /* CONFIG_ST_HAL_ACCEL_CALIB_ENABLED */
|
|
|
|
memcpy(tmp_raw_data, data->raw, SENSOR_DATA_3AXIS * sizeof(float));
|
|
|
|
data->raw[0] = SENSOR_X_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_ACCEL_ROT_MATRIX);
|
|
data->raw[1] = SENSOR_Y_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_ACCEL_ROT_MATRIX);
|
|
data->raw[2] = SENSOR_Z_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_ACCEL_ROT_MATRIX);
|
|
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_EXTRA_VERBOSE)
|
|
ALOGD("\"%s\": received new sensor data: x=%f y=%f z=%f, timestamp=%" PRIu64 "ns, deltatime=%" PRIu64 "ns (sensor type: %d).",
|
|
sensor_t_data.name, data->raw[0], data->raw[1], data->raw[2],
|
|
data->timestamp, data->timestamp - sensor_event.timestamp, sensor_t_data.type);
|
|
#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */
|
|
|
|
#ifdef CONFIG_ST_HAL_FACTORY_CALIBRATION
|
|
data->raw[0] = (data->raw[0] - factory_offset[0]) * factory_scale[0];
|
|
data->raw[1] = (data->raw[1] - factory_offset[1]) * factory_scale[1];
|
|
data->raw[2] = (data->raw[2] - factory_offset[2]) * factory_scale[2];
|
|
data->accuracy = SENSOR_STATUS_ACCURACY_HIGH;
|
|
#else /* CONFIG_ST_HAL_FACTORY_CALIBRATION */
|
|
data->accuracy = SENSOR_STATUS_UNRELIABLE;
|
|
#endif /* CONFIG_ST_HAL_FACTORY_CALIBRATION */
|
|
|
|
#ifdef CONFIG_ST_HAL_ACCEL_CALIB_ENABLED
|
|
acc_cal_input.timestamp = data->timestamp;
|
|
acc_cal_input.acc_raw[0] = data->raw[0];
|
|
acc_cal_input.acc_raw[1] = data->raw[1];
|
|
acc_cal_input.acc_raw[2] = data->raw[2];
|
|
|
|
ST_AccCalibration_API_Run(&acc_cal_output, &acc_cal_input);
|
|
|
|
data->processed[0] = acc_cal_output.acc_cal[0];
|
|
data->processed[1] = acc_cal_output.acc_cal[1];
|
|
data->processed[2] = acc_cal_output.acc_cal[2];
|
|
data->offset[0] = acc_cal_output.offset[0];
|
|
data->offset[1] = acc_cal_output.offset[1];
|
|
data->offset[2] = acc_cal_output.offset[2];
|
|
data->accuracy = acc_cal_output.accuracy;
|
|
#if (CONFIG_ST_HAL_DEBUG_LEVEL >= ST_HAL_DEBUG_EXTRA_VERBOSE)
|
|
ALOGD("\"%s\": ACCEL-CALIB (accuracy %d - offs: x=%f y=%f z=%f) received new sensor data: x=%f y=%f z=%f (norm is %f), timestamp=%" PRIu64 "ns, deltatime=%" PRIu64 "ns (sensor type: %d).",
|
|
sensor_t_data.name, data->accuracy, data->offset[0], data->offset[1], data->offset[2], data->processed[0], data->processed[1], data->processed[2],
|
|
sqrtf(data->processed[0]*data->processed[0] + data->processed[1]*data->processed[1] + data->processed[2]*data->processed[2]), data->timestamp, data->timestamp - sensor_event.timestamp, sensor_t_data.type);
|
|
#endif /* CONFIG_ST_HAL_DEBUG_LEVEL */
|
|
#else /* CONFIG_ST_HAL_ACCEL_CALIB_ENABLED */
|
|
data->processed[0] = data->raw[0];
|
|
data->processed[1] = data->raw[1];
|
|
data->processed[2] = data->raw[2];
|
|
|
|
data->accuracy = SENSOR_STATUS_UNRELIABLE;
|
|
#endif /* CONFIG_ST_HAL_ACCEL_CALIB_ENABLED */
|
|
|
|
sensor_event.acceleration.x = data->processed[0];
|
|
sensor_event.acceleration.y = data->processed[1];
|
|
sensor_event.acceleration.z = data->processed[2];
|
|
sensor_event.acceleration.status = data->accuracy;
|
|
sensor_event.timestamp = data->timestamp;
|
|
|
|
HWSensorBaseWithPollrate::WriteDataToPipe(data->pollrate_ns);
|
|
HWSensorBaseWithPollrate::ProcessData(data);
|
|
}
|
|
|
|
|
|
#if (CONFIG_ST_HAL_ANDROID_VERSION >= ST_HAL_PIE_VERSION)
|
|
#if (CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED)
|
|
int Accelerometer::getSensorAdditionalInfoPayLoadFramesArray(additional_info_event_t **array_sensorAdditionalInfoPLFrames)
|
|
{
|
|
additional_info_event_t* p_custom_SAI_Placement_event = nullptr;
|
|
|
|
// place for ODM/OEM to fill custom_SAI_Placement_event
|
|
// p_custom_SAI_Placement_event = &custom_SAI_Placement_event
|
|
|
|
/* //Custom Placement example
|
|
additional_info_event_t custom_SAI_Placement_event;
|
|
custom_SAI_Placement_event = {
|
|
.type = AINFO_SENSOR_PLACEMENT,
|
|
.serial = 0,
|
|
.data_float = {-1, 0, 0, 1, 0, -1, 0, 2, 0, 0, -1, 3},
|
|
};
|
|
p_custom_SAI_Placement_event = &custom_SAI_Placement_event;
|
|
*/
|
|
|
|
return UseCustomAINFOSensorPlacementPLFramesArray(array_sensorAdditionalInfoPLFrames, p_custom_SAI_Placement_event);
|
|
|
|
}
|
|
#endif /* CONFIG_ST_HAL_ADDITIONAL_INFO_ENABLED */
|
|
#endif /* CONFIG_ST_HAL_ANDROID_VERSION */
|