From ef9f1053d05d95fba0efae016ce4acc97d2d745a Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 17 Dec 2019 18:53:19 +0100 Subject: [PATCH] Random improvements --- EKF/control.cpp | 18 ++++++++++++------ EKF/ekf_helper.cpp | 28 ++++++++++++++-------------- 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 5ae4da6ba6..76b865acbe 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -57,22 +57,28 @@ void Ekf::controlFusionModes() // and declare the tilt alignment complete if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) { _control_status.flags.tilt_align = true; - _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); + _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState()); // TODO: is this needed? + // send alignment status message to the console if (_control_status.flags.baro_hgt) { - ECL_INFO("%llu: EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", + (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else if (_control_status.flags.ev_hgt) { - ECL_INFO("%llu: EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", + (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else if (_control_status.flags.gps_hgt) { - ECL_INFO("%llu: EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", + (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else if (_control_status.flags.rng_hgt) { - ECL_INFO("%llu: EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", + (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else { - ECL_ERR("%llu: EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_ERR("%llu: EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", + (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7ed2ddf63d..0671e4d820 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -592,18 +592,18 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool Dcmf R_to_earth(euler321); // calculate the observed yaw angle - if (_control_status.flags.ev_yaw) { - // convert the observed quaternion to a rotation matrix - Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame - // calculate the yaw angle for a 312 sequence - euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); - - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle euler321(2) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + } else if (_control_status.flags.ev_yaw) { + // convert the observed quaternion to a rotation matrix + Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0)); + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { // we are operating without knowing the earth frame yaw angle return true; @@ -650,18 +650,18 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool R_to_earth(2, 1) = s1; // calculate the observed yaw angle - if (_control_status.flags.ev_yaw) { - // convert the observed quaternion to a rotation matrix - Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame - // calculate the yaw angle for a 312 sequence - euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); - - } else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { + if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { // rotate the magnetometer measurements into earth frame using a zero yaw angle Vector3f mag_earth_pred = R_to_earth * mag_init; // the angle of the projection onto the horizontal gives the yaw angle euler312(0) = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + } else if (_control_status.flags.ev_yaw) { + // convert the observed quaternion to a rotation matrix + Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame + // calculate the yaw angle for a 312 sequence + euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1)); + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _mag_use_inhibit) { // we are operating without knowing the earth frame yaw angle return true;