m_gyro_sensor = sensor_plugin_loader::get_instance().get_sensor(GYROSCOPE_SENSOR);
m_magnetic_sensor = sensor_plugin_loader::get_instance().get_sensor(GEOMAGNETIC_SENSOR);
- if (!m_accel_sensor || !m_gyro_sensor || !m_magnetic_sensor) {
- ERR("Failed to load sensors, accel: 0x%x, gyro: 0x%x, mag: 0x%x",
- m_accel_sensor, m_gyro_sensor, m_magnetic_sensor);
+ if (!m_accel_sensor) {
+ ERR("Failed to load accel sensor: 0x%x", m_accel_sensor);
return false;
}
+ if (!m_gyro_sensor)
+ INFO("Failed to load gyro sensor: 0x%x", m_gyro_sensor);
+
+ if (!m_magnetic_sensor)
+ INFO("Failed to load geomagnetic sensor: 0x%x", m_magnetic_sensor);
+
INFO("%s is created!", sensor_base::get_name());
return true;
}
sensor_data_t magnetic_data;
euler_angles<float> euler_orientation;
- float azimuth_offset;
if (event_type != FUSION_ORIENTATION_ENABLED ||
event_type != FUSION_ROTATION_VECTOR_ENABLED ||
{
initialize_sensor_data(accel, gyro, magnetic);
- if (magnetic != NULL)
+ if (gyro != NULL && magnetic != NULL) {
+
orientation_triad_algorithm();
- else if (gyro != NULL)
- compute_accel_orientation();
- if (gyro != NULL) {
compute_covariance();
- if (magnetic != NULL)
- time_update();
- else
- time_update_gaming_rv();
+ time_update();
measurement_update();
- if (magnetic == NULL) {
- m_quaternion = m_quat_gaming_rv;
- } else {
- m_quaternion = m_quat_9axis;
- }
+ m_quaternion = m_quat_9axis;
+
+ } else if (!gyro) {
+
+ orientation_triad_algorithm();
- } else {
m_quaternion = m_quat_aid;
+
+ } else if (!magnetic) {
+
+ compute_accel_orientation();
+
+ compute_covariance();
+
+ time_update_gaming_rv();
+
+ measurement_update();
+
+ m_quaternion = m_quat_gaming_rv;
}
}
orientation.m_ang.m_vec[0] *= pitch_phase_compensation;
orientation.m_ang.m_vec[1] *= roll_phase_compensation;
- orientation.m_ang.m_vec[2] *= azimuth_phase_compensation;
+ orientation.m_ang.m_vec[2] *= azimuth_phase_compensation;
if (orientation.m_ang.m_vec[2] < 0)
orientation.m_ang.m_vec[2] += 360;
static GMainLoop *mainloop;
FILE *fp;
-void callback(unsigned int event_type, sensor_event_data_t *event, void *user_data)
+void callback(sensor_t sensor, unsigned int event_type, sensor_data_t *data, void *user_data)
{
g_main_loop_quit(mainloop);
}
<DEFAULT_SAMPLING_TIME value="100" />
</GAMING_ROTATION_VECTOR>
</DEVICE>
-
+
<DEVICE type="Mobile-RD-PQ_V2">
<FUSION>
<NAME value="FUSION_SENSOR" />