m_orientation_filter.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
m_orientation_filter.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
- quaternion_orientation = m_orientation_filter.get_9axis_quaternion(m_accel, m_gyro, m_magnetic);
+ quaternion_orientation = m_orientation_filter.get_9axis_quaternion(&m_accel, &m_gyro, &m_magnetic);
m_time = get_timestamp();
rv_event.sensor_id = get_id();
m_orientation_filter_poll.m_azimuth_phase_compensation = m_azimuth_rotation_compensation;
m_orientation_filter_poll.m_magnetic_alignment_factor = m_magnetic_alignment_factor;
- quaternion_orientation = m_orientation_filter_poll.get_9axis_quaternion(m_accel, m_gyro, m_magnetic);
+ quaternion_orientation = m_orientation_filter_poll.get_9axis_quaternion(&m_accel, &m_gyro, &m_magnetic);
data.accuracy = SENSOR_ACCURACY_GOOD;
data.timestamp = get_timestamp();
}
template <typename TYPE>
-inline void orientation_filter<TYPE>::init_accel_gyro_mag_data(const sensor_data<TYPE> accel,
- const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
-{
- unsigned long long sample_interval_gyro = SAMPLE_INTV;
-
- m_accel.m_data = accel.m_data;
- m_magnetic.m_data = magnetic.m_data;
-
- if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0)
- sample_interval_gyro = gyro.m_time_stamp - m_gyro.m_time_stamp;
-
- m_gyro_dt = sample_interval_gyro * US2S;
-
- m_accel.m_time_stamp = accel.m_time_stamp;
- m_gyro.m_time_stamp = gyro.m_time_stamp;
- m_magnetic.m_time_stamp = magnetic.m_time_stamp;
-
- m_gyro.m_data = gyro.m_data - m_bias_correction;
-}
-
-template <typename TYPE>
inline void orientation_filter<TYPE>::init_accel_mag_data(const sensor_data<TYPE> accel,
const sensor_data<TYPE> magnetic)
{
}
template <typename TYPE>
-quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_data<TYPE> accel,
- const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
+quaternion<TYPE> orientation_filter<TYPE>::get_9axis_quaternion(const sensor_data<TYPE> *accel,
+ const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic)
{
- get_orientation(&accel, &gyro, &magnetic);
+ get_device_rotation(accel, gyro, magnetic);
return m_quat_9axis;
}
const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
rotation_matrix<TYPE> get_rotation_matrix(const sensor_data<TYPE> *accel,
const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
- quaternion<TYPE> get_9axis_quaternion(const sensor_data<TYPE> accel,
- const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
+ quaternion<TYPE> get_9axis_quaternion(const sensor_data<TYPE> *accel,
+ const sensor_data<TYPE> *gyro, const sensor_data<TYPE> *magnetic);
quaternion<TYPE> get_geomagnetic_quaternion(const sensor_data<TYPE> accel,
const sensor_data<TYPE> magnetic);
quaternion<TYPE> get_gaming_quaternion(const sensor_data<TYPE> accel,
return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data);
}
-quaternion<float> orientation_sensor::get_9axis_quaternion(sensor_data<float> accel_data,
- sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
+quaternion<float> orientation_sensor::get_9axis_quaternion(sensor_data<float> *accel_data,
+ sensor_data<float> *gyro_data, sensor_data<float> *magnetic_data)
{
- pre_process_data(&accel_data, &accel_data, bias_accel, sign_accel, scale_accel);
- normalize(accel_data);
- pre_process_data(&gyro_data, &gyro_data, bias_gyro, sign_gyro, scale_gyro);
- pre_process_data(&magnetic_data, &magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
- normalize(magnetic_data);
+ pre_process_data(accel_data, accel_data, bias_accel, sign_accel, scale_accel);
+ normalize(*accel_data);
+ pre_process_data(gyro_data, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+ pre_process_data(magnetic_data, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
+ normalize(*magnetic_data);
orien_filter.m_pitch_phase_compensation = pitch_phase_compensation;
orien_filter.m_roll_phase_compensation = roll_phase_compensation;
sensor_data<float> *gyro, sensor_data<float> *magnetic);
rotation_matrix<float> get_rotation_matrix(sensor_data<float> *accel,
sensor_data<float> *gyro, sensor_data<float> *magnetic);
- quaternion<float> get_9axis_quaternion(sensor_data<float> accel,
- sensor_data<float> gyro, sensor_data<float> magnetic);
+ quaternion<float> get_9axis_quaternion(sensor_data<float> *accel,
+ sensor_data<float> *gyro, sensor_data<float> *magnetic);
quaternion<float> get_geomagnetic_quaternion(sensor_data<float> accel,
sensor_data<float> magnetic);
quaternion<float> get_gaming_quaternion(sensor_data<float> accel,
cout << "Orientation matrix\t" << orientation_mat.m_rot_mat << "\n\n";
- orientation_9axis_quat = orien_sensor3.get_9axis_quaternion(accel_data, gyro_data, magnetic_data);
+ orientation_9axis_quat = orien_sensor3.get_9axis_quaternion(&accel_data, &gyro_data, &magnetic_data);
cout << "Orientation 9-axis quaternion\t" << orientation_9axis_quat.m_quat << "\n\n";