Random improvements

PX4 · Dec 18, 2019 · ef9f105 · ef9f105
1 parent 7d3814b
commit ef9f105
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 20 deletions.
diff --git 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
@@ -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;