#define QWB_CONST ((2 * (ZIGMA_W * ZIGMA_W)) / TAU_W)
#define F_CONST (-1 / TAU_W)
-#define BIAS_AX 0.098586
-#define BIAS_AY 0.18385
-#define BIAS_AZ (10.084 - GRAVITY)
-
-#define BIAS_GX -5.3539
-#define BIAS_GY 0.24325
-#define BIAS_GZ 2.3391
-
-#define DRIVING_SYSTEM_PHASE_COMPENSATION -1
-
-#define SCALE_GYRO 575
-
#define ENABLE_LPF false
#define M3X3R 3
m_state_new = vec1x6;
m_state_old = vec1x6;
m_state_error = vec1x6;
+
+ m_pitch_phase_compensation = 1;
+ m_roll_phase_compensation = 1;
+ m_yaw_phase_compensation = 1;
+ m_magnetic_alignment_factor = 1;
}
template <typename TYPE>
const TYPE iir_b[] = {0.98, 0};
const TYPE iir_a[] = {1.0000000, 0.02};
- TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ};
- TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ};
-
vect<TYPE> acc_data(V1x3S);
vect<TYPE> gyr_data(V1x3S);
- vect<TYPE> acc_bias(V1x3S, a_bias);
- vect<TYPE> gyr_bias(V1x3S, g_bias);
-
- acc_data = accel.m_data - acc_bias;
- gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO;
m_filt_accel[0] = m_filt_accel[1];
m_filt_gyro[0] = m_filt_gyro[1];
if (ENABLE_LPF)
{
- m_filt_accel[1].m_data = acc_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1];
- m_filt_gyro[1].m_data = gyr_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1];
+ m_filt_accel[1].m_data = accel.m_data * iir_b[0] - m_filt_accel[0].m_data * iir_a[1];
+ m_filt_gyro[1].m_data = gyro.m_data * iir_b[0] - m_filt_gyro[0].m_data * iir_a[1];
m_filt_magnetic[1].m_data = magnetic.m_data * iir_b[0] - m_filt_magnetic[0].m_data * iir_a[1];
}
else
{
- m_filt_accel[1].m_data = acc_data;
- m_filt_gyro[1].m_data = gyr_data;
+ m_filt_accel[1].m_data = accel.m_data;
+ m_filt_gyro[1].m_data = gyro.m_data;
m_filt_magnetic[1].m_data = magnetic.m_data;
}
m_filt_gyro[1].m_time_stamp = accel.m_time_stamp;
m_filt_magnetic[1].m_time_stamp = accel.m_time_stamp;
- normalize(m_filt_accel[1]);
- normalize(m_filt_magnetic[1]);
-
m_filt_gyro[1].m_data = m_filt_gyro[1].m_data - m_bias_correction;
}
inline void orientation_filter<TYPE>::orientation_triad_algorithm()
{
TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
- TYPE arr_mag_e[V1x3S] = {0.0, -1.0, 0.0};
+ TYPE arr_mag_e[V1x3S] = {0.0, m_magnetic_alignment_factor, 0.0};
vect<TYPE> acc_e(V1x3S, arr_acc_e);
vect<TYPE> mag_e(V1x3S, arr_mag_e);
orientation = quat2euler(m_quat_driv);
- m_orientation = orientation.m_ang * (TYPE) DRIVING_SYSTEM_PHASE_COMPENSATION;
+ m_orientation.m_ang.m_vec[0] = orientation.m_ang.m_vec[0] * m_roll_phase_compensation;
+ m_orientation.m_ang.m_vec[1] = orientation.m_ang.m_vec[1] * m_pitch_phase_compensation;
+ m_orientation.m_ang.m_vec[2] = orientation.m_ang.m_vec[2] * m_yaw_phase_compensation;
m_rot_matrix = quat2rot_mat(m_quat_driv);
rotation_matrix<TYPE> m_rot_matrix;
euler_angles<TYPE> m_orientation;
+ int m_pitch_phase_compensation;
+ int m_roll_phase_compensation;
+ int m_yaw_phase_compensation;
+ int m_magnetic_alignment_factor;
+
orientation_filter();
~orientation_filter();
#ifdef _ORIENTATION_SENSOR_H
-euler_angles<float> orientation_sensor::get_orientation(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic)
+float bias_accel[] = {0.098586, 0.18385, (10.084 - GRAVITY)};
+float bias_gyro[] = {-5.3539, 0.24325, 2.3391};
+float bias_magnetic[] = {0, 0, 0};
+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 = 575;
+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<float> &data_in, float *bias, int *sign, float scale)
+{
+ data_out.m_data.m_vec[0] = sign[0] * (data_in.m_data.m_vec[0] - bias[0]) / scale;
+ data_out.m_data.m_vec[1] = sign[1] * (data_in.m_data.m_vec[1] - bias[1]) / scale;
+ data_out.m_data.m_vec[2] = sign[2] * (data_in.m_data.m_vec[2] - bias[2]) / scale;
+
+ data_out.m_time_stamp = data_in.m_time_stamp;
+}
+
+euler_angles<float> orientation_sensor::get_orientation(sensor_data<float> accel_data,
+ sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
{
- return orien_filter.get_orientation(accel, gyro, magnetic);
+
+ 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;
+ orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
+ orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+ return orien_filter.get_orientation(accel_data, gyro_data, magnetic_data);
}
-rotation_matrix<float> orientation_sensor::get_rotation_matrix(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic)
+rotation_matrix<float> orientation_sensor::get_rotation_matrix(sensor_data<float> accel_data,
+ sensor_data<float> gyro_data, sensor_data<float> magnetic_data)
{
- return orien_filter.get_rotation_matrix(accel, gyro, magnetic);
+
+ 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;
+ orien_filter.m_yaw_phase_compensation = yaw_phase_compensation;
+ orien_filter.m_magnetic_alignment_factor = magnetic_alignment_factor;
+
+ return orien_filter.get_rotation_matrix(accel_data, gyro_data, magnetic_data);
}
#endif
public:
orientation_filter<float> orien_filter;
- euler_angles<float> get_orientation(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic);
- rotation_matrix<float> get_rotation_matrix(const sensor_data<float> accel,
- const sensor_data<float> gyro, const sensor_data<float> magnetic);
+ euler_angles<float> get_orientation(sensor_data<float> accel,
+ 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);
};
#include "orientation_sensor.cpp"