void setup_ahrs(void)
{
/* Set up IMU. */
setup_mpu();
/* Initialize DCMs as identity matrices. */
static uint8_t i, j;
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
m_init_identity(dcm_gyro);
m_init_identity(dcm_offset);
m_init_identity(dcm_d);
}
}
/**
* Calculate DCM offset.
*
* We use an accelerometer to correct for gyro drift. This means that the
* IMU will adjust dcm_gyro according to whatever the accelerometer thinks
* is gravity. However, since it is nearly impossible to mount the
* accelerometer perfectly horizontal to the chassis, this also means that
* dcm_gyro will represent the orientation of the accelerometer, not the
* chassis, along the X and Y rotational axes.
*
* The rotational difference between the orientations of the accelerometer
* (dcm_gyro) and the chassis (dcm_out) is constant and can be approximated
* by another rotation matrix we will call dcm_offset, which we populate with
* trim angles obtained during hover calibration. We rotate dcm_gyro by
* dcm_offset to obtain dcm_out.
*
* Keep in mind, however, that we still update the IMU based on dcm_gyro and
* not dcm_out. This is because it is dcm_gyro we are correcting with the
* accelerometer's measurement of the gravity vector, and dcm_out is only
* a transformation of that DCM based on dcm_offset. We use dcm_out for
* flight calculations, but dcm_gyro is what we keep track of within the
* IMU.
*
* As a final note, this is a small-angle approximation! We could do
* something fancy with trigonometry, but if the hardware is so poorly
* built (or poorly designed) that small-angle approximations become
* insufficient, we can't expect software to fix everything.
*/
#ifdef ACC_WEIGHT
// TODO: Set trim angles.
trim_angle[0] = TRIM_ANGLE_X;
trim_angle[1] = TRIM_ANGLE_Y;
v_acc[0] = 0;
v_acc[1] = 0;
v_acc[2] = 1;
for (i=0; i<3; i++) {
v_acc_last[i] = v_acc[i];
}
//accVar = 100.;
#endif // ACC_WEIGHT
}
static void process_noise_covariance(
const kalman_robot_handle_t * handle,
float delta_t,
covariance_t * dest)
{
float base_factor =
handle->_process_noise_proportionality * handle->_max_acc;
m_init_identity(&(dest->_cov_a));
m_init_identity(&(dest->_cov_b));
m_init_identity(&(dest->_cov_c));
m_init_identity(&(dest->_cov_d));
float factor = 0.25f * delta_t * delta_t * delta_t * delta_t;
m_scalar_mult(factor * base_factor, &(dest->_cov_a), &(dest->_cov_a));
factor = 0.5f * delta_t * delta_t * delta_t;
m_scalar_mult(factor * base_factor, &(dest->_cov_b), &(dest->_cov_b));
m_scalar_mult(factor * base_factor, &(dest->_cov_c), &(dest->_cov_c));
factor = delta_t * delta_t;
m_scalar_mult(factor * base_factor, &(dest->_cov_d), &(dest->_cov_d));
}
