#define ACCELEROMETER_ENABLED 0x01
#define GYROSCOPE_ENABLED 0x02
#define GEOMAGNETIC_ENABLED 0x04
-#define ORIENTATION_ENABLED 0x07
+#define ORIENTATION_ENABLED 7
+
#define INITIAL_VALUE -1
#define INITIAL_TIME 0
-#define SAMPLING_TIME 0.1
+// Below Defines const variables to be input from sensor config files once code is stabilized
+#define SAMPLING_TIME 100
#define MS_TO_US 1000
-void copy_sensor_data(sensor_data<float> &data_out, sensor_data_t &data_in)
+float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
+float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
+float bias_magnetic[] = {0, -37.6, +37.6};
+int sign_accel[] = {+1, +1, +1};
+int sign_gyro[] = {+1, +1, +1};
+int sign_magnetic[] = {+1, -1, +1};
+float scale_accel = 1;
+float scale_gyro = 580 * 2;
+float scale_magnetic = 1;
+
+int pitch_phase_compensation = 1;
+int roll_phase_compensation = 1;
+int yaw_phase_compensation = -1;
+int magnetic_alignment_factor = 1;
+
+void pre_process_data(sensor_data<float> &data_out, sensor_data_t &data_in, float *bias, int *sign, float scale)
{
- data_out.m_data.m_vec[0] = data_in.values[0];
- data_out.m_data.m_vec[1] = data_in.values[1];
- data_out.m_data.m_vec[2] = data_in.values[2];
+ data_out.m_data.m_vec[0] = sign[0] * (data_in.values[0] - bias[0]) / scale;
+ data_out.m_data.m_vec[1] = sign[1] * (data_in.values[1] - bias[1]) / scale;
+ data_out.m_data.m_vec[2] = sign[2] * (data_in.values[2] - bias[2]) / scale;
+
data_out.m_time_stamp = data_in.timestamp;
}
{
m_name = string(SENSOR_NAME);
register_supported_event(ORIENTATION_EVENT_RAW_DATA_REPORT_ON_TIME);
- m_timestamp = SAMPLING_TIME * MS_TO_US;
+ m_interval = SAMPLING_TIME * MS_TO_US;
+ m_timestamp = get_timestamp();
+ m_enable_orientation = 0;
}
orientation_sensor::~orientation_sensor()
if (event.event_type == ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME) {
diff_time = event.data.timestamp - m_timestamp;
- if (m_timestamp && (diff_time < SAMPLING_TIME * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
+
+ if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
- copy_sensor_data(accel, accel_data);
+ pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
m_enable_orientation |= ACCELEROMETER_ENABLED;
}
else if (event.event_type == GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME) {
diff_time = event.data.timestamp - m_timestamp;
- if (m_timestamp && (diff_time < SAMPLING_TIME * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
+
+ if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
- copy_sensor_data(gyro, gyro_data);
+ pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
m_enable_orientation |= GYROSCOPE_ENABLED;
}
else if (event.event_type == GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME) {
diff_time = event.data.timestamp - m_timestamp;
- if (m_timestamp && (diff_time < SAMPLING_TIME * MS_TO_US * MIN_DELIVERY_DIFF_FACTOR))
+
+ if (m_timestamp && (diff_time < m_interval * MIN_DELIVERY_DIFF_FACTOR))
return;
- m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, mag_data);
- copy_sensor_data(magnetic, mag_data);
+ pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
m_enable_orientation |= GEOMAGNETIC_ENABLED;
}
m_enable_orientation = 0;
m_timestamp = get_timestamp();
+ m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
+ m_orientation.m_roll_phase_compensation = roll_phase_compensation;
+ m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
+ m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
orientation_event.data.data_accuracy = SENSOR_ACCURACY_GOOD;
orientation_event.data.values[0] = euler_orientation.m_ang.m_vec[0];
orientation_event.data.values[1] = euler_orientation.m_ang.m_vec[1];
orientation_event.data.values[2] = euler_orientation.m_ang.m_vec[2];
+
outs.push_back(orientation_event);
}
sensor_data_t accel_data;
sensor_data_t gyro_data;
- sensor_data_t mag_data;
+ sensor_data_t magnetic_data;
euler_angles<float> euler_orientation;
m_accel_sensor->get_sensor_data(ACCELEROMETER_EVENT_RAW_DATA_REPORT_ON_TIME, accel_data);
m_gyro_sensor->get_sensor_data(GYROSCOPE_EVENT_RAW_DATA_REPORT_ON_TIME, gyro_data);
- m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, mag_data);
+ m_magnetic_sensor->get_sensor_data(GEOMAGNETIC_EVENT_RAW_DATA_REPORT_ON_TIME, magnetic_data);
+
+ pre_process_data(accel, accel_data, bias_accel, sign_accel, scale_accel);
+ pre_process_data(gyro, gyro_data, bias_gyro, sign_gyro, scale_gyro);
+ pre_process_data(magnetic, magnetic_data, bias_magnetic, sign_magnetic, scale_magnetic);
- copy_sensor_data(accel, accel_data);
- copy_sensor_data(gyro, gyro_data);
- copy_sensor_data(magnetic, mag_data);
+ m_orientation.m_pitch_phase_compensation = pitch_phase_compensation;
+ m_orientation.m_roll_phase_compensation = roll_phase_compensation;
+ m_orientation.m_yaw_phase_compensation = yaw_phase_compensation;
+ m_orientation.m_magnetic_alignment_factor = magnetic_alignment_factor;
euler_orientation = m_orientation.get_orientation(accel, gyro, magnetic);
data.values[1] = euler_orientation.m_ang.m_vec[1];
data.values[2] = euler_orientation.m_ang.m_vec[2];
data.values_num = 3;
+
return 0;
}
properties.sensor_min_range = -180;
properties.sensor_max_range = 180;
properties.sensor_resolution = 1;
+
strncpy(properties.sensor_vendor, "Samsung", MAX_KEY_LENGTH);
strncpy(properties.sensor_name, SENSOR_NAME, MAX_KEY_LENGTH);
+
return true;
}