#define ELEMENT_ACCEL_ROTATION_DIRECTION_COMPENSATION "ACCEL_ROTATION_DIRECTION_COMPENSATION"
#define ELEMENT_GYRO_ROTATION_DIRECTION_COMPENSATION "GYRO_ROTATION_DIRECTION_COMPENSATION"
#define ELEMENT_GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION "GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION"
-#define ELEMENT_ACCEL_SCALE "ACCEL_SCALE"
-#define ELEMENT_GYRO_SCALE "GYRO_SCALE"
-#define ELEMENT_GEOMAGNETIC_SCALE "GEOMAGNETIC_SCALE"
#define ELEMENT_MAGNETIC_ALIGNMENT_FACTOR "MAGNETIC_ALIGNMENT_FACTOR"
#define ELEMENT_PITCH_ROTATION_COMPENSATION "PITCH_ROTATION_COMPENSATION"
#define ELEMENT_ROLL_ROTATION_COMPENSATION "ROLL_ROTATION_COMPENSATION"
INFO("m_geomagnetic_rotation_direction_compensation = (%d, %d, %d)", m_geomagnetic_rotation_direction_compensation[0], m_geomagnetic_rotation_direction_compensation[1], m_geomagnetic_rotation_direction_compensation[2]);
- if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
- ERR("[ACCEL_SCALE] is empty\n");
- throw ENXIO;
- }
-
- INFO("m_accel_scale = %f", m_accel_scale);
-
- if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GYRO_SCALE, &m_gyro_scale)) {
- ERR("[GYRO_SCALE] is empty\n");
- throw ENXIO;
- }
-
- INFO("m_gyro_scale = %f", m_gyro_scale);
-
- if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_GEOMAGNETIC_SCALE, &m_geomagnetic_scale)) {
- ERR("[GEOMAGNETIC_SCALE] is empty\n");
- throw ENXIO;
- }
-
- INFO("m_geomagnetic_scale = %f", m_geomagnetic_scale);
-
if (!config.get(SENSOR_TYPE_FUSION, ELEMENT_MAGNETIC_ALIGNMENT_FACTOR, &m_magnetic_alignment_factor)) {
ERR("[MAGNETIC_ALIGNMENT_FACTOR] is empty\n");
throw ENXIO;
if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
+ pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
m_accel.m_time_stamp = event.data.timestamp;
if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
+ pre_process_data(m_magnetic, event.data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, GEOMAGNETIC_SCALE);
m_magnetic.m_time_stamp = event.data.timestamp;
if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
+ pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
m_gyro.m_time_stamp = event.data.timestamp;
return -1;
m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
- pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
+ pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
accel.m_time_stamp = accel_data.timestamp;
if (event_type == FUSION_ORIENTATION_ENABLED ||
event_type == FUSION_GYROSCOPE_UNCAL_ENABLED)
{
m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data);
- pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
+ pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
gyro.m_time_stamp = gyro_data.timestamp;
}
event_type == FUSION_GYROSCOPE_UNCAL_ENABLED)
{
m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_RAW_DATA_EVENT, magnetic_data);
- pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, m_geomagnetic_scale);
+ pre_process_data(magnetic, magnetic_data.values, m_geomagnetic_static_bias, m_geomagnetic_rotation_direction_compensation, GEOMAGNETIC_SCALE);
magnetic.m_time_stamp = magnetic_data.timestamp;
}
int m_accel_rotation_direction_compensation[3];
int m_gyro_rotation_direction_compensation[3];
int m_geomagnetic_rotation_direction_compensation[3];
- float m_accel_scale;
- float m_gyro_scale;
- float m_geomagnetic_scale;
int m_magnetic_alignment_factor;
bool on_start(void);
INFO("m_accel_rotation_direction_compensation = (%d, %d, %d)", m_accel_rotation_direction_compensation[0], m_accel_rotation_direction_compensation[1], m_accel_rotation_direction_compensation[2]);
-
- if (!config.get(SENSOR_TYPE_LINEAR_ACCEL, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
- ERR("[ACCEL_SCALE] is empty\n");
- throw ENXIO;
- }
-
- INFO("m_accel_scale = %f", m_accel_scale);
-
if (!config.get(SENSOR_TYPE_GRAVITY, ELEMENT_GRAVITY_SIGN_COMPENSATION, m_gravity_sign_compensation, 3)) {
ERR("[GRAVITY_SIGN_COMPENSATION] is empty\n");
throw ENXIO;
if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- m_accel.m_data.m_vec[0] = m_accel_rotation_direction_compensation[0] * (event.data.values[0] - m_accel_static_bias[0]) / m_accel_scale;
- m_accel.m_data.m_vec[1] = m_accel_rotation_direction_compensation[1] * (event.data.values[1] - m_accel_static_bias[1]) / m_accel_scale;
- m_accel.m_data.m_vec[2] = m_accel_rotation_direction_compensation[2] * (event.data.values[2] - m_accel_static_bias[2]) / m_accel_scale;
+ m_accel.m_data.m_vec[0] = m_accel_rotation_direction_compensation[0] * (event.data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE;
+ m_accel.m_data.m_vec[1] = m_accel_rotation_direction_compensation[1] * (event.data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE;
+ m_accel.m_data.m_vec[2] = m_accel_rotation_direction_compensation[2] * (event.data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE;
m_accel.m_time_stamp = event.data.timestamp;
gravity_data = calculate_gravity(fusion_data);
- accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / m_accel_scale;
- accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / m_accel_scale;
- accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / m_accel_scale;
+ accel_data.values[0] = m_accel_rotation_direction_compensation[0] * (accel_data.values[0] - m_accel_static_bias[0]) / ACCEL_SCALE;
+ accel_data.values[1] = m_accel_rotation_direction_compensation[1] * (accel_data.values[1] - m_accel_static_bias[1]) / ACCEL_SCALE;
+ accel_data.values[2] = m_accel_rotation_direction_compensation[2] * (accel_data.values[2] - m_accel_static_bias[2]) / ACCEL_SCALE;
if (event_type != LINEAR_ACCEL_RAW_DATA_EVENT)
return -1;
int m_default_sampling_time;
float m_accel_static_bias[3];
int m_accel_rotation_direction_compensation[3];
- float m_accel_scale;
int m_linear_accel_sign_compensation[3];
int m_gravity_sign_compensation[3];
int m_azimuth_rotation_compensation;
INFO("m_gyro_rotation_direction_compensation = (%d, %d, %d)", m_gyro_rotation_direction_compensation[0], m_gyro_rotation_direction_compensation[1], m_gyro_rotation_direction_compensation[2]);
- if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_ACCEL_SCALE, &m_accel_scale)) {
- ERR("[ACCEL_SCALE] is empty\n");
- throw ENXIO;
- }
-
- INFO("m_accel_scale = %f", m_accel_scale);
-
- if (!config.get(SENSOR_TYPE_GAMING_RV, ELEMENT_GYRO_SCALE, &m_gyro_scale)) {
- ERR("[GYRO_SCALE] is empty\n");
- throw ENXIO;
- }
-
- INFO("m_gyro_scale = %f", m_gyro_scale);
-
m_interval = m_default_sampling_time * MS_TO_US;
}
if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
+ pre_process_data(m_accel, event.data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
m_accel.m_time_stamp = event.data.timestamp;
if (m_time && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
+ pre_process_data(m_gyro, event.data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
m_gyro.m_time_stamp = event.data.timestamp;
m_accel_sensor->get_sensor_data(ACCELEROMETER_RAW_DATA_EVENT, accel_data);
m_gyro_sensor->get_sensor_data(GYROSCOPE_RAW_DATA_EVENT, gyro_data);
- pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, m_accel_scale);
- pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, m_gyro_scale);
+ pre_process_data(accel, accel_data.values, m_accel_static_bias, m_accel_rotation_direction_compensation, ACCEL_SCALE);
+ pre_process_data(gyro, gyro_data.values, m_gyro_static_bias, m_gyro_rotation_direction_compensation, GYRO_SCALE);
accel.m_time_stamp = accel_data.timestamp;
gyro.m_time_stamp = gyro_data.timestamp;
float m_gyro_static_bias[3];
int m_accel_rotation_direction_compensation[3];
int m_gyro_rotation_direction_compensation[3];
- float m_accel_scale;
- float m_gyro_scale;
-
bool on_start(void);
bool on_stop(void);
};
#define V1x3S 3
#define V1x4S 4
#define V1x6S 6
+#define ACCEL_SCALE 1
+#define GYRO_SCALE 1146
+#define GEOMAGNETIC_SCALE 1
template <typename TYPE>
class orientation_filter {
}
template<typename T>
-void pre_process_data(sensor_data<T> &data_out, const T *data_in, T *bias, int *sign, T scale)
+void pre_process_data(sensor_data<T> &data_out, const T *data_in, T *bias, int *sign, int scale)
{
data_out.m_data.m_vec[0] = sign[0] * (data_in[0] - bias[0]) / scale;
data_out.m_data.m_vec[1] = sign[1] * (data_in[1] - bias[1]) / scale;
template<typename T> friend quaternion<T> sensor_data2quat(const sensor_data<T> data,
const vect<T, REF_VEC_SIZE> ref_vec);
template<typename T> friend void pre_process_data(sensor_data<T> &data_out,
- const T *data_in, T *bias, int *sign, T scale);
+ const T *data_in, T *bias, int *sign, int scale);
};
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="-1" value3="-1" />
<GYRO_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
<GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="-1" value3="-1" />
- <ACCEL_SCALE value="1" />
- <GYRO_SCALE value="1146" />
- <GEOMAGNETIC_SCALE value="1" />
<MAGNETIC_ALIGNMENT_FACTOR value="1" />
</FUSION>
<DEFAULT_SAMPLING_TIME value="100" />
<ACCEL_STATIC_BIAS value1="0" value2="0" value3="0" />
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="-1" value3="-1" />
- <ACCEL_SCALE value="1" />
<LINEAR_ACCEL_SIGN_COMPENSATION value1="1" value2="1" value3="1" />
</LINEAR_ACCEL>
<GYRO_STATIC_BIAS value1="0" value2="0" value3="0" />
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="-1" value3="-1" />
<GYRO_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
- <ACCEL_SCALE value="1" />
- <GYRO_SCALE value="1146" />
</GAMING_ROTATION_VECTOR>
<TILT>
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
<GYRO_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
<GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
- <ACCEL_SCALE value="1" />
- <GYRO_SCALE value="1150" />
- <GEOMAGNETIC_SCALE value="1" />
<MAGNETIC_ALIGNMENT_FACTOR value="1" />
</FUSION>
<DEFAULT_SAMPLING_TIME value="100" />
<ACCEL_STATIC_BIAS value1="0" value2="0" value3="0" />
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
- <ACCEL_SCALE value="1" />
<LINEAR_ACCEL_SIGN_COMPENSATION value1="1" value2="1" value3="1" />
</LINEAR_ACCEL>
<GYRO_STATIC_BIAS value1="3.5638" value2="-5.8288" value3="-0.79750" />
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
<GYRO_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
- <ACCEL_SCALE value="1" />
- <GYRO_SCALE value="1150" />
</GAMING_ROTATION_VECTOR>
<TILT>
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="-1" value3="1" />
<GYRO_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="1" value3="-1" />
<GEOMAGNETIC_ROTATION_DIRECTION_COMPENSATION value1="1" value2="-1" value3="1" />
- <ACCEL_SCALE value="1" />
- <GYRO_SCALE value="1150" />
- <GEOMAGNETIC_SCALE value="1" />
<MAGNETIC_ALIGNMENT_FACTOR value="-1" />
</FUSION>
<DEFAULT_SAMPLING_TIME value="100" />
<ACCEL_STATIC_BIAS value1="0" value2="0" value3="0" />
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="1" value3="1" />
- <ACCEL_SCALE value="1" />
<LINEAR_ACCEL_SIGN_COMPENSATION value1="1" value2="1" value3="1" />
</LINEAR_ACCEL>
<GYRO_STATIC_BIAS value1="-0.54385" value2="0.24325" value3="0.87875" />
<ACCEL_ROTATION_DIRECTION_COMPENSATION value1="1" value2="-1" value3="1" />
<GYRO_ROTATION_DIRECTION_COMPENSATION value1="-1" value2="1" value3="-1" />
- <ACCEL_SCALE value="1" />
- <GYRO_SCALE value="1150" />
</GAMING_ROTATION_VECTOR>
<TILT>