void aos_compute_sensors(void)
{
struct FloatRates gyro;
RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
// printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));
float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);
RATES_BFP_OF_REAL(imu.gyro, gyro);
RATES_BFP_OF_REAL(imu.gyro_prev, gyro);
struct FloatVect3 g_ltp = {0., 0., 9.81};
struct FloatVect3 accelero_ltp;
VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
struct FloatVect3 accelero_imu;
float_quat_vmult(&accelero_imu, &aos.ltp_to_imu_quat, &accelero_ltp);
float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);
#ifndef DISABLE_MAG_UPDATE
struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 h_imu;
float_quat_vmult(&h_imu, &aos.ltp_to_imu_quat, &h_earth);
MAGS_BFP_OF_REAL(imu.mag, h_imu);
#endif
aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR2
ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR
ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.gps_age = 0;
ahrs_update_gps();
//RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration));
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel));
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#endif
}
static void handle_ins_msg(void)
{
update_state_interface();
if (xsens.new_attitude) {
new_ins_attitude = true;
xsens.new_attitude = false;
}
#if USE_GPS_XSENS
if (xsens.gps_available) {
// Horizontal speed
float fspeed = FLOAT_VECT2_NORM(xsens.vel);
if (xsens.gps.fix != GPS_FIX_3D) {
fspeed = 0;
}
xsens.gps.gspeed = fspeed * 100.;
xsens.gps.speed_3d = float_vect3_norm(&xsens.vel) * 100;
float fcourse = atan2f(xsens.vel.y, xsens.vel.x);
xsens.gps.course = fcourse * 1e7;
SetBit(xsens.gps.valid_fields, GPS_VALID_COURSE_BIT);
gps_xsens_publish();
xsens.gps_available = false;
}
#endif // USE_GPS_XSENS
}
void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt)
{
// check if we had at least one propagation since last update
if (ahrs_fc.accel_cnt == 0) {
return;
}
/* last column of roation matrix = ltp z-axis in imu-frame */
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
};
struct FloatVect3 imu_accel_float;
ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel);
struct FloatVect3 residual;
struct FloatVect3 pseudo_gravity_measurement;
if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) {
/*
* centrifugal acceleration in body frame
* a_c_body = omega x (omega x r)
* (omega x r) = tangential velocity in body frame
* a_c_body = omega x vel_tangential_body
* assumption: tangential velocity only along body x-axis
*/
const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0};
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu);
struct FloatRates body_rate;
float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate);
struct FloatVect3 acc_c_body;
VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body);
/* convert centrifugal acceleration from body to imu frame */
struct FloatVect3 acc_c_imu;
float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body);
/* and subtract it from imu measurement to get a corrected measurement of the gravity vector */
VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu);
} else {
VECT3_COPY(pseudo_gravity_measurement, imu_accel_float);
}
VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2);
/* FIR filtered pseudo_gravity_measurement */
#define FIR_FILTER_SIZE 8
static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.};
VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1);
VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement);
VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE);
if (ahrs_fc.gravity_heuristic_factor) {
/* heuristic on acceleration (gravity estimate) norm */
/* Factor how strongly to change the weight.
* e.g. for gravity_heuristic_factor 30:
* <0.66G = 0, 1G = 1.0, >1.33G = 0
*/
const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81;
ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0;
Bound(ahrs_fc.weight, 0.15, 1.0);
} else {
ahrs_fc.weight = 1.0;
}
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt
* with ahrs_fc.accel_cnt beeing the number of propagations since last update
*/
const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain);
// reset accel propagation counter
ahrs_fc.accel_cnt = 0;
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2 * dt
*/
const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega *
ahrs_fc.weight * ahrs_fc.weight * dt / 9.81;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain);
/* FIXME: saturate bias */
}