1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
|
/*
* STMicroelectronics Gyroscope Sensor Class
*
* Copyright 2013-2015 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 "sensor_cal.h"
#include "Gyroscope.h"
#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
extern "C" {
#include "iNemoEngineAPI_gbias_estimation.h"
}
#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
Gyroscope::Gyroscope(HWSensorBaseCommonData *data, const char *name,
struct iio_sampling_frequency_available *sfa, int handle,
unsigned int hw_fifo_len, int pipe_data_fd, float power_consumption, bool wakeup) :
HWSensorBaseWithPollrate(data, name, sfa, handle,
SENSOR_TYPE_GYROSCOPE, hw_fifo_len, pipe_data_fd, power_consumption)
{
sensor_t_data.stringType = SENSOR_STRING_TYPE_GYROSCOPE;
sensor_t_data.flags = SENSOR_FLAG_CONTINUOUS_MODE;
if (wakeup)
sensor_t_data.flags |= SENSOR_FLAG_WAKE_UP;
sensor_t_data.resolution = data->channels[0].scale;
sensor_t_data.maxRange = sensor_t_data.resolution * (pow(2.0, data->channels[0].bits_used - 1.0) - 1);
#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
iNemoEngine_API_gbias_Initialization(NULL);
type_dependencies[SENSOR_BASE_DEPENDENCY_0] = SENSOR_TYPE_ACCELEROMETER;
#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
}
Gyroscope::~Gyroscope()
{
}
int Gyroscope::Enable(int handle, bool enable)
{
#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
int err;
bool old_status;
old_status = GetStatus();
err = HWSensorBaseWithPollrate::Enable(handle, enable);
if (err < 0)
return err;
if (GetStatus() != old_status)
iNemoEngine_API_gbias_enable(enable);
return 0;
#else /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
return HWSensorBaseWithPollrate::Enable(handle, enable);
#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
}
int Gyroscope::SetDelay(int handle, int64_t period_ns, int64_t timeout)
{
#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
int err;
err = HWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout);
if (err < 0)
return err;
iNemoEngine_API_gbias_set_frequency(NS_TO_FREQUENCY(GetRealPollrate()));
return 0;
#else /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
return HWSensorBaseWithPollrate::SetDelay(handle, period_ns, timeout);
#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
}
void Gyroscope::ProcessData(SensorBaseData *data)
{
#define X_SCALING_FACTOR 1
#define Y_SCALING_FACTOR (360 / 350)
#define Z_SCALING_FACTOR 1
float tmp_raw_data[num_data_axis];
#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
int64_t time_diff = 0;
int err, nomaxdata = 10;
SensorBaseData accel_data;
#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
memcpy(tmp_raw_data, data->raw, num_data_axis * sizeof(float));
data->raw[0] = SENSOR_X_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_GYRO_ROT_MATRIX);
data->raw[1] = SENSOR_Y_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_GYRO_ROT_MATRIX);
data->raw[2] = SENSOR_Z_DATA(tmp_raw_data[0], tmp_raw_data[1], tmp_raw_data[2], CONFIG_ST_HAL_GYRO_ROT_MATRIX);
#ifdef CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED
do {
err = GetLatestValidDataFromDependency(SENSOR_BASE_DEPENDENCY_0, &accel_data);
if (err < 0) {
nomaxdata--;
usleep(200);
continue;
}
time_diff = data->timestamp - accel_data.timestamp;
} while ((time_diff >= GetRealPollrate()) && (nomaxdata > 0));
if (err >= 0)
iNemoEngine_API_gbias_Run(accel_data.raw, data->raw);
iNemoEngine_API_Get_gbias(data->offset);
sensor_event.gyro.status = SENSOR_STATUS_ACCURACY_HIGH;
#else /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
if (gyro_cal_data_loaded == true)
sensor_event.gyro.status = SENSOR_STATUS_ACCURACY_LOW;
else
sensor_event.gyro.status = SENSOR_STATUS_UNRELIABLE;
#endif /* CONFIG_ST_HAL_GYRO_GBIAS_ESTIMATION_ENABLED */
data->processed[0] = data->raw[0] - data->offset[0];
data->processed[1] = data->raw[1] - data->offset[1];
data->processed[2] = data->raw[2] - data->offset[2];
/* FIXME: scaling gyro data for better accuracy */
sensor_event.gyro.x = data->processed[0] * X_SCALING_FACTOR;
sensor_event.gyro.y = data->processed[1] * Y_SCALING_FACTOR;
sensor_event.gyro.z = data->processed[2] * Z_SCALING_FACTOR;
sensor_event.timestamp = data->timestamp;
HWSensorBaseWithPollrate::WriteDataToPipe();
HWSensorBaseWithPollrate::ProcessData(data);
}
|
