feat : caculate rightinclination and compare current inclination

이충섭
Commit f8ff5ebdc6505e1f02e9e9af5c50a5461f0ec9ad f8ff5ebd 1 parent 1fd483af
Showing 2 changed files with 20 additions and 435 deletions
home/Firmware/src/modules/ekf2/EKF2.cpp
home/Firmware/src/modules/ekf2/EKF2.hpp
--- a/home/Firmware/src/modules/ekf2/EKF2.cpp
View file @f8ff5eb
+++ b/home/Firmware/src/modules/ekf2/EKF2.cpp
View file @f8ff5eb
@@ -30,7 +30,6 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 #include "EKF2.hpp"
 #include <iostream>
 #include <px4_platform_common/defines.h>
@@ -165,7 +164,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
 {
 }
-
 EKF2::~EKF2()
 {
 	perf_free(_ecl_ekf_update_perf);
@@ -173,7 +171,6 @@ EKF2::~EKF2()
 	perf_free(_imu_missed_perf);
 	perf_free(_mag_missed_perf);
 }
-
 bool EKF2::multi_init(int imu, int mag)
 {
 	// advertise immediately to ensure consistent uORB instance numbering
@@ -182,7 +179,6 @@ bool EKF2::multi_init(int imu, int mag)
 	_global_position_pub.advertise();
 	_odometry_pub.advertise();
 	_wind_pub.advertise();
-
 	_ekf2_timestamps_pub.advertise();
 	_ekf_gps_drift_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
@@ -195,28 +191,20 @@ bool EKF2::multi_init(int imu, int mag)
 	_estimator_status_flags_pub.advertise();
 	_estimator_visual_odometry_aligned_pub.advertised();
 	_yaw_est_pub.advertise();
-
 	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
-
 	const int status_instance = _estimator_states_pub.get_instance();
-
 	if ((status_instance >= 0) && changed_instance
 	    && (_attitude_pub.get_instance() == status_instance)
 	    && (_local_position_pub.get_instance() == status_instance)
 	    && (_global_position_pub.get_instance() == status_instance)) {
-
 		_instance = status_instance;
-
 		ScheduleNow();
 		return true;
 	}
-
 	PX4_ERR("publication instance problem: %d att: %d lpos: %d gpos: %d", status_instance,
 		_attitude_pub.get_instance(), _local_position_pub.get_instance(), _global_position_pub.get_instance());
-
 	return false;
 }
-
 int EKF2::print_status()
 {
 	PX4_INFO_RAW("ekf2:%d attitude: %d, local position: %d, global position: %d\n", _instance, _ekf.attitude_valid(),
@@ -224,71 +212,54 @@ int EKF2::print_status()
 	perf_print_counter(_ecl_ekf_update_perf);
 	perf_print_counter(_ecl_ekf_update_full_perf);
 	perf_print_counter(_imu_missed_perf);
-
 	if (_device_id_mag != 0) {
 		perf_print_counter(_mag_missed_perf);
 	}
-
 	return 0;
 }
-
 void EKF2::Run()
 {
 	if (should_exit()) {
 		_sensor_combined_sub.unregisterCallback();
 		_vehicle_imu_sub.unregisterCallback();
-
 		return;
 	}
-
 	// check for parameter updates
 	if (_parameter_update_sub.updated() || !_callback_registered) {
 		// clear update
 		parameter_update_s pupdate;
 		_parameter_update_sub.copy(&pupdate);
-
 		// update parameters from storage
 		updateParams();
-
 		_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
-
 		// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
 		param_t param_aspd_scale = param_find("ASPD_SCALE");
-
 		if (param_aspd_scale != PARAM_INVALID) {
 			param_get(param_aspd_scale, &_airspeed_scale_factor);
 		}
 	}
-
 	if (!_callback_registered) {
 		if (_multi_mode) {
 			_callback_registered = _vehicle_imu_sub.registerCallback();
-
 		} else {
 			_callback_registered = _sensor_combined_sub.registerCallback();
 		}
-
 		if (!_callback_registered) {
 			PX4_WARN("%d - failed to register callback, retrying", _instance);
 			ScheduleDelayed(1_s);
 			return;
 		}
 	}
-
 	if (_vehicle_command_sub.updated()) {
 		vehicle_command_s vehicle_command;
-
 		if (_vehicle_command_sub.update(&vehicle_command)) {
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
 				if (!_ekf.control_status_flags().in_air) {
-
 					uint64_t origin_time {};
 					double latitude = vehicle_command.param5;
 					double longitude = vehicle_command.param6;
 					float altitude = vehicle_command.param7;
-
 					_ekf.setEkfGlobalOrigin(latitude, longitude, altitude);
-
 					// Validate the ekf origin status.
 					_ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
 					PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
@@ -296,81 +267,63 @@ void EKF2::Run()
 			}
 		}
 	}
-
 	bool imu_updated = false;
 	imuSample imu_sample_new {};
-
 	hrt_abstime imu_dt = 0; // for tracking time slip later
-
 	if (_multi_mode) {
 		const unsigned last_generation = _vehicle_imu_sub.get_last_generation();
 		vehicle_imu_s imu;
 		imu_updated = _vehicle_imu_sub.update(&imu);
-
 		if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
 			perf_count(_imu_missed_perf);
 			PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
 				  _vehicle_imu_sub.get_last_generation());
 		}
-
 		imu_sample_new.time_us = imu.timestamp_sample;
 		imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;
 		imu_sample_new.delta_ang = Vector3f{imu.delta_angle};
 		imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f;
 		imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
-
 		if (imu.delta_velocity_clipping > 0) {
 			imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
 			imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
 			imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
 		}
-
 		imu_dt = imu.delta_angle_dt;
-
 		if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
 			_device_id_accel = imu.accel_device_id;
 			_device_id_gyro = imu.gyro_device_id;
 			_imu_calibration_count = imu.calibration_count;
-
 		} else if ((imu.calibration_count > _imu_calibration_count)
 			   || (imu.accel_device_id != _device_id_accel)
 			   || (imu.gyro_device_id != _device_id_gyro)) {
-
 			PX4_INFO("%d - resetting IMU bias", _instance);
 			_device_id_accel = imu.accel_device_id;
 			_device_id_gyro = imu.gyro_device_id;
-
 			_ekf.resetImuBias();
 			_imu_calibration_count = imu.calibration_count;
 		}
-
 	} else {
 		sensor_combined_s sensor_combined;
 		imu_updated = _sensor_combined_sub.update(&sensor_combined);
-
 		imu_sample_new.time_us = sensor_combined.timestamp;
 		imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f;
 		imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt;
 		imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
 		imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
-
 		if (sensor_combined.accelerometer_clipping > 0) {
 			imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X;
 			imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y;
 			imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z;
 		}
-
 		imu_dt = sensor_combined.gyro_integral_dt;
-
 		if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) {
 			sensor_selection_s sensor_selection;
-
 			if (_sensor_selection_sub.copy(&sensor_selection)) {
 				if (_device_id_accel != sensor_selection.accel_device_id) {
 					_ekf.resetAccelBias();
 					_device_id_accel = sensor_selection.accel_device_id;
 				}
-
 				if (_device_id_gyro != sensor_selection.gyro_device_id) {
 					_ekf.resetGyroBias();
 					_device_id_gyro = sensor_selection.gyro_device_id;
@@ -378,72 +331,54 @@ void EKF2::Run()
 			}
 		}
 	}
-
 	if (imu_updated) {
 		const hrt_abstime now = imu_sample_new.time_us;
-
 		// push imu data into estimator
 		_ekf.setIMUData(imu_sample_new);
 		PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor)
-
 		// integrate time to monitor time slippage
 		if (_start_time_us > 0) {
 			_integrated_time_us += imu_dt;
 			_last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us;
-
 		} else {
 			_start_time_us = imu_sample_new.time_us;
 			_last_time_slip_us = 0;
 		}
-
 		// update all other topics if they have new data
 		if (_status_sub.updated()) {
 			vehicle_status_s vehicle_status;
-
 			if (_status_sub.copy(&vehicle_status)) {
 				const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-
 				// only fuse synthetic sideslip measurements if conditions are met
 				_ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
-
 				// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
 				_ekf.set_is_fixed_wing(is_fixed_wing);
-
 				_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
 						vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
-
 				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-
 				// update standby (arming state) flag
 				const bool standby = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
-
 				if (_standby != standby) {
 					_standby = standby;
-
 					// reset preflight checks if transitioning in or out of standby arming state
 					_preflt_checker.reset();
 				}
 			}
 		}
-
 		if (_vehicle_land_detected_sub.updated()) {
 			vehicle_land_detected_s vehicle_land_detected;
-
 			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
 				_ekf.set_in_air_status(!vehicle_land_detected.landed);
-
 				if (_armed && (_param_ekf2_gnd_eff_dz.get() > 0.f)) {
 					if (!_had_valid_terrain) {
 						// update ground effect flag based on land detector state if we've never had valid terrain data
 						_ekf.set_gnd_effect_flag(vehicle_land_detected.in_ground_effect);
 					}
-
 				} else {
 					_ekf.set_gnd_effect_flag(false);
 				}
 			}
 		}
-
 		// ekf2_timestamps (using 0.1 ms relative timestamps)
 		ekf2_timestamps_s ekf2_timestamps {
 			.timestamp = now,
@@ -454,33 +389,25 @@ void EKF2::Run()
 			.vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 			.visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 		};
-
 		UpdateAirspeedSample(ekf2_timestamps);
 		UpdateAuxVelSample(ekf2_timestamps);
 		UpdateBaroSample(ekf2_timestamps);
 		UpdateGpsSample(ekf2_timestamps);
 		UpdateMagSample(ekf2_timestamps);
 		UpdateRangeSample(ekf2_timestamps);
-
 		vehicle_odometry_s ev_odom;
 		const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
-
 		optical_flow_s optical_flow;
 		const bool new_optical_flow = UpdateFlowSample(ekf2_timestamps, optical_flow);
-
-
 		// run the EKF update and output
 		const hrt_abstime ekf_update_start = hrt_absolute_time();
-
 		if (_ekf.update()) {
 			perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
-
 			PublishLocalPosition(now);
 			PublishOdometry(now, imu_sample_new);
 			PublishGlobalPosition(now);
 			PublishSensorBias(now);
 			PublishWindEstimate(now);
-
 			// publish status/logging messages
 			PublishEkfDriftMetrics(now);
 			PublishEventFlags(now);
@@ -491,28 +418,22 @@ void EKF2::Run()
 			PublishInnovationTestRatios(now);
 			PublishInnovationVariances(now);
 			PublishYawEstimatorStatus(now);
-
 			UpdateMagCalibration(now);
-
 		} else {
 			// ekf no update
 			perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
 		}
-
 		// publish external visual odometry after fixed frame alignment if new odometry is received
 		if (new_ev_odom) {
 			PublishOdometryAligned(now, ev_odom);
 		}
-
 		if (new_optical_flow) {
 			PublishOpticalFlowVel(now, optical_flow);
 		}
-
 		// publish ekf2_timestamps
 		_ekf2_timestamps_pub.publish(ekf2_timestamps);
 	}
 }
-
 void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 {
 	if (_ekf.attitude_valid()) {
@@ -521,11 +442,9 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 		att.timestamp_sample = timestamp;
 		const Quatf q{_ekf.calculate_quaternion()};
 		q.copyTo(att.q);
-
 		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
 		att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_attitude_pub.publish(att);
-
 	}  else if (_replay_mode) {
 		// in replay mode we have to tell the replay module not to wait for an update
 		// we do this by publishing an attitude with zero timestamp
@@ -533,13 +452,11 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 		_attitude_pub.publish(att);
 	}
 }
-
 void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
 {
 	// publish GPS drift data only when updated to minimise overhead
 	float gps_drift[3];
 	bool blocked;
-
 	if (_ekf.get_gps_drift_metrics(gps_drift, &blocked)) {
 		ekf_gps_drift_s drift_data;
 		drift_data.hpos_drift_rate = gps_drift[0];
@@ -547,35 +464,28 @@ void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
 		drift_data.hspd = gps_drift[2];
 		drift_data.blocked = blocked;
 		drift_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-
 		_ekf_gps_drift_pub.publish(drift_data);
 	}
 }
-
 void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 {
 	// information events
 	uint32_t information_events = _ekf.information_event_status().value;
 	bool information_event_updated = false;
-
 	if (information_events != 0) {
 		information_event_updated = true;
 		_filter_information_event_changes++;
 	}
-
 	// warning events
 	uint32_t warning_events = _ekf.warning_event_status().value;
 	bool warning_event_updated = false;
-
 	if (warning_events != 0) {
 		warning_event_updated = true;
 		_filter_warning_event_changes++;
 	}
-
 	if (information_event_updated || warning_event_updated) {
 		estimator_event_flags_s event_flags{};
 		event_flags.timestamp_sample = timestamp;
-
 		event_flags.information_event_changes           = _filter_information_event_changes;
 		event_flags.gps_checks_passed                   = _ekf.information_event_flags().gps_checks_passed;
 		event_flags.reset_vel_to_gps                    = _ekf.information_event_flags().reset_vel_to_gps;
@@ -590,7 +500,6 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 		event_flags.starting_vision_vel_fusion          = _ekf.information_event_flags().starting_vision_vel_fusion;
 		event_flags.starting_vision_yaw_fusion          = _ekf.information_event_flags().starting_vision_yaw_fusion;
 		event_flags.yaw_aligned_to_imu_gps              = _ekf.information_event_flags().yaw_aligned_to_imu_gps;
-
 		event_flags.warning_event_changes               = _filter_warning_event_changes;
 		event_flags.gps_quality_poor                    = _ekf.warning_event_flags().gps_quality_poor;
 		event_flags.gps_fusion_timout                   = _ekf.warning_event_flags().gps_fusion_timout;
@@ -603,62 +512,48 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
 		event_flags.stopping_mag_use                    = _ekf.warning_event_flags().stopping_mag_use;
 		event_flags.vision_data_stopped                 = _ekf.warning_event_flags().vision_data_stopped;
 		event_flags.emergency_yaw_reset_mag_stopped     = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
-
 		event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_event_flags_pub.publish(event_flags);
 	}
-
 	_ekf.clear_information_events();
 	_ekf.clear_warning_events();
 }
-
 void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 {
 	if (_ekf.global_position_is_valid() && !_preflt_checker.hasFailed()) {
 		// only publish if position has changed by at least 1 mm (map_projection_reproject is relatively expensive)
 		const Vector3f position{_ekf.getPosition()};
-
 		if ((_last_local_position_for_gpos - position).longerThan(0.001f)) {
 			// generate and publish global position data
 			vehicle_global_position_s global_pos;
 			global_pos.timestamp_sample = timestamp;
-
 			// Position of local NED origin in GPS / WGS84 frame
 			map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon);
-
 			float delta_xy[2];
 			_ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter);
-
 			global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
 			global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
-
 			// global altitude has opposite sign of local down position
 			float delta_z;
 			uint8_t z_reset_counter;
 			_ekf.get_posD_reset(&delta_z, &z_reset_counter);
 			global_pos.delta_alt = -delta_z;
-
 			_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
-
 			if (_ekf.isTerrainEstimateValid()) {
 				// Terrain altitude in m, WGS84
 				global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
 				global_pos.terrain_alt_valid = true;
-
 			} else {
 				global_pos.terrain_alt = NAN;
 				global_pos.terrain_alt_valid = false;
 			}
-
 			global_pos.dead_reckoning = _ekf.inertial_dead_reckoning(); // True if this position is estimated through dead-reckoning
 			global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 			_global_position_pub.publish(global_pos);
-
 			_last_local_position_for_gpos = position;
 		}
 	}
 }
-
 void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu)
 {
 	// publish estimator innovation data
@@ -678,10 +573,8 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu
 	_ekf.getHaglInnov(innovations.hagl);
 	// Not yet supported
 	innovations.aux_vvel = NAN;
-
 	innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovations_pub.publish(innovations);
-
 	// calculate noise filtered velocity innovations which are used for pre-flight checking
 	if (_standby) {
 		// TODO: move to run before publications
@@ -689,11 +582,9 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp, const imuSample &imu
 		_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
 		_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
 		_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
-
 		_preflt_checker.update(imu.delta_ang_dt, innovations);
 	}
 }
-
 void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 {
 	// publish estimator innovation test ratio data
@@ -714,11 +605,9 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
 	_ekf.getHaglInnovRatio(test_ratios.hagl);
 	// Not yet supported
 	test_ratios.aux_vvel = NAN;
-
 	test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_test_ratios_pub.publish(test_ratios);
 }
-
 void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 {
 	// publish estimator innovation variance data
@@ -738,44 +627,36 @@ void EKF2::PublishInnovationVariances(const hrt_abstime &timestamp)
 	_ekf.getHaglInnovVar(variances.hagl);
 	// Not yet supported
 	variances.aux_vvel = NAN;
-
 	variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_innovation_variances_pub.publish(variances);
 }
-
 void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 {
 	vehicle_local_position_s lpos;
 	// generate vehicle local position data
 	lpos.timestamp_sample = timestamp;
-
 	// Position of body origin in local NED frame
 	const Vector3f position{_ekf.getPosition()};
 	lpos.x = position(0);
 	lpos.y = position(1);
 	lpos.z = position(2);
-
 	// Velocity of body origin in local NED frame (m/s)
 	const Vector3f velocity{_ekf.getVelocity()};
 	lpos.vx = velocity(0);
 	lpos.vy = velocity(1);
 	lpos.vz = velocity(2);
-
 	// vertical position time derivative (m/s)
 	lpos.z_deriv = _ekf.getVerticalPositionDerivative();
-
 	// Acceleration of body origin in local frame
 	const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
 	lpos.ax = vel_deriv(0);
 	lpos.ay = vel_deriv(1);
 	lpos.az = vel_deriv(2);
-
 	// TODO: better status reporting
 	lpos.xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
 	lpos.z_valid = !_preflt_checker.hasVertFailed();
 	lpos.v_xy_valid = _ekf.local_position_is_valid() && !_preflt_checker.hasHorizFailed();
 	lpos.v_z_valid = !_preflt_checker.hasVertFailed();
-
 	// Position of local NED origin in GPS / WGS84 frame
 	if (_ekf.global_origin_valid()) {
 		lpos.ref_timestamp = _ekf.global_origin().timestamp;
@@ -784,7 +665,6 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 		lpos.ref_alt = _ekf.getEkfGlobalOriginAltitude();           // Reference point in MSL altitude meters
 		lpos.xy_global = true;
 		lpos.z_global = true;
-
 	} else {
 		lpos.ref_timestamp = 0;
 		lpos.ref_lat = static_cast<double>(NAN);
@@ -793,147 +673,115 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 		lpos.xy_global = false;
 		lpos.z_global = false;
 	}
-
 	Quatf delta_q_reset;
 	_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
-
 	lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
 	lpos.delta_heading = Eulerf(delta_q_reset).psi();
-
 	// Distance to bottom surface (ground) in meters
 	// constrain the distance to ground to _rng_gnd_clearance
 	lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get());
 	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
-
 	if (!_had_valid_terrain) {
 		_had_valid_terrain = lpos.dist_bottom_valid;
 	}
-
 	// only consider ground effect if compensation is configured and the vehicle is armed (props spinning)
 	if ((_param_ekf2_gnd_eff_dz.get() > 0.0f) && _armed && lpos.dist_bottom_valid) {
 		// set ground effect flag if vehicle is closer than a specified distance to the ground
 		_ekf.set_gnd_effect_flag(lpos.dist_bottom < _param_ekf2_gnd_max_hgt.get());
-
 		// if we have no valid terrain estimate and never had one then use ground effect flag from land detector
 		// _had_valid_terrain is used to make sure that we don't fall back to using this option
 		// if we temporarily lose terrain data due to the distance sensor getting out of range
 	}
-
 	_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
 	_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
-
 	// get state reset information of position and velocity
 	_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
 	_ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
 	_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
 	_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
-
 	// get control limit information
 	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
-
 	// convert NaN to INFINITY
 	if (!PX4_ISFINITE(lpos.vxy_max)) {
 		lpos.vxy_max = INFINITY;
 	}
-
 	if (!PX4_ISFINITE(lpos.vz_max)) {
 		lpos.vz_max = INFINITY;
 	}
-
 	if (!PX4_ISFINITE(lpos.hagl_min)) {
 		lpos.hagl_min = INFINITY;
 	}
-
 	if (!PX4_ISFINITE(lpos.hagl_max)) {
 		lpos.hagl_max = INFINITY;
 	}
-
 	// publish vehicle local position data
 	lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_local_position_pub.publish(lpos);
 }
-
 void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 {
 	// generate vehicle odometry data
 	vehicle_odometry_s odom;
 	odom.timestamp_sample = imu.time_us;
-
 	odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
-
 	// Vehicle odometry position
 	const Vector3f position{_ekf.getPosition()};
 	odom.x = position(0);
 	odom.y = position(1);
 	odom.z = position(2);
-
 	// Vehicle odometry linear velocity
 	odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
 	const Vector3f velocity{_ekf.getVelocity()};
 	odom.vx = velocity(0);
 	odom.vy = velocity(1);
 	odom.vz = velocity(2);
-
 	// Vehicle odometry quaternion
 	_ekf.getQuaternion().copyTo(odom.q);
-
 	// Vehicle odometry angular rates
 	const Vector3f gyro_bias{_ekf.getGyroBias()};
 	const Vector3f rates{imu.delta_ang / imu.delta_ang_dt};
 	odom.rollspeed = rates(0) - gyro_bias(0);
 	odom.pitchspeed = rates(1) - gyro_bias(1);
 	odom.yawspeed = rates(2) - gyro_bias(2);
-
 	// get the covariance matrix size
 	static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
 	static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
-
 	// Get covariances to vehicle odometry
 	float covariances[24];
 	_ekf.covariances_diagonal().copyTo(covariances);
-
 	// initially set pose covariances to 0
 	for (size_t i = 0; i < POS_URT_SIZE; i++) {
 		odom.pose_covariance[i] = 0.0;
 	}
-
 	// set the position variances
 	odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
 	odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
 	odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
-
 	// TODO: implement propagation from quaternion covariance to Euler angle covariance
 	// by employing the covariance law
-
 	// initially set velocity covariances to 0
 	for (size_t i = 0; i < VEL_URT_SIZE; i++) {
 		odom.velocity_covariance[i] = 0.0;
 	}
-
 	// set the linear velocity variances
 	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
 	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
 	odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
-
 	// publish vehicle odometry data
 	odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_odometry_pub.publish(odom);
 }
-
 void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_odometry_s &ev_odom)
 {
 	const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame
 	const Dcmf ev_rot_mat(quat_ev2ekf);
-
 	vehicle_odometry_s aligned_ev_odom{ev_odom};
-
 	// Rotate external position and velocity into EKF navigation frame
 	const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z);
 	aligned_ev_odom.x = aligned_pos(0);
 	aligned_ev_odom.y = aligned_pos(1);
 	aligned_ev_odom.z = aligned_pos(2);
-
 	switch (ev_odom.velocity_frame) {
 	case vehicle_odometry_s::BODY_FRAME_FRD: {
 			const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
@@ -942,7 +790,6 @@ void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_od
 			aligned_ev_odom.vz = aligned_vel(2);
 			break;
 		}
-
 	case vehicle_odometry_s::LOCAL_FRAME_FRD: {
 			const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
 			aligned_ev_odom.vx = aligned_vel(0);
@@ -951,34 +798,26 @@ void EKF2::PublishOdometryAligned(const hrt_abstime &timestamp, const vehicle_od
 			break;
 		}
 	}
-
 	aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
-
 	// Compute orientation in EKF navigation frame
 	Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q) ;
 	ev_quat_aligned.normalize();
-
 	ev_quat_aligned.copyTo(aligned_ev_odom.q);
 	quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
-
 	_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
 }
-
 void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 {
 	// estimator_sensor_bias
 	estimator_sensor_bias_s bias{};
 	bias.timestamp_sample = timestamp;
-
 	const Vector3f gyro_bias{_ekf.getGyroBias()};
 	const Vector3f accel_bias{_ekf.getAccelBias()};
 	const Vector3f mag_bias{_mag_cal_last_bias};
-
 	// only publish on change
 	if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
 	    || (accel_bias - _last_accel_bias_published).longerThan(0.001f)
 	    || (mag_bias - _last_mag_bias_published).longerThan(0.001f)) {
-
 		// take device ids from sensor_selection_s if not using specific vehicle_imu_s
 		if (_device_id_gyro != 0) {
 			bias.gyro_device_id = _device_id_gyro;
@@ -986,35 +825,28 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 			bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates()
 			_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
 			bias.gyro_bias_valid = true;
-
 			_last_gyro_bias_published = gyro_bias;
 		}
-
 		if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
 			bias.accel_device_id = _device_id_accel;
 			accel_bias.copyTo(bias.accel_bias);
 			bias.accel_bias_limit = _params->acc_bias_lim;
 			_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
 			bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias;
-
 			_last_accel_bias_published = accel_bias;
 		}
-
 		if (_device_id_mag != 0) {
 			bias.mag_device_id = _device_id_mag;
 			mag_bias.copyTo(bias.mag_bias);
 			bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates()
 			_mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance);
 			bias.mag_bias_valid = _mag_cal_available;
-
 			_last_mag_bias_published = mag_bias;
 		}
-
 		bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_sensor_bias_pub.publish(bias);
 	}
 }
-
 void EKF2::PublishStates(const hrt_abstime &timestamp)
 {
 	// publish estimator states
@@ -1026,91 +858,72 @@ void EKF2::PublishStates(const hrt_abstime &timestamp)
 	states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_states_pub.publish(states);
 }
-
 void EKF2::PublishStatus(const hrt_abstime &timestamp)
 {
 	estimator_status_s status{};
 	status.timestamp_sample = timestamp;
-
 	_ekf.getOutputTrackingError().copyTo(status.output_tracking_error);
-
 	_ekf.get_gps_check_status(&status.gps_check_fail_flags);
-
 	// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
 	// the GPS Fix bit, which is always checked)
 	status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1;
-
 	status.control_mode_flags = _ekf.control_status().value;
 	status.filter_fault_flags = _ekf.fault_status().value;
-
 	uint16_t innov_check_flags_temp = 0;
 	_ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio,
 					status.vel_test_ratio, status.pos_test_ratio,
 					status.hgt_test_ratio, status.tas_test_ratio,
 					status.hagl_test_ratio, status.beta_test_ratio);
-
 	// Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition
 	// TODO: legacy use only, those flags are also in estimator_status_flags
 	status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1);
-
 	_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
 	_ekf.get_ekf_soln_status(&status.solution_status_flags);
 	_ekf.getImuVibrationMetrics().copyTo(status.vibe);
-
 	// reset counters
 	status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
 	status.reset_count_vel_d = _ekf.state_reset_status().velD_counter;
 	status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter;
 	status.reset_count_pod_d = _ekf.state_reset_status().posD_counter;
 	status.reset_count_quat = _ekf.state_reset_status().quat_counter;
-
 	status.time_slip = _last_time_slip_us * 1e-6f;
-
 	status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
 	status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
 	status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
 	status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
 	status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
-
 	status.accel_device_id = _device_id_accel;
 	status.baro_device_id = _device_id_baro;
 	status.gyro_device_id = _device_id_gyro;
 	status.mag_device_id = _device_id_mag;
-
 	status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 	_estimator_status_pub.publish(status);
 }
-
 void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 {
 	// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
 	bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s);
-
 	// filter control status
 	if (_ekf.control_status().value != _filter_control_status) {
 		update = true;
 		_filter_control_status = _ekf.control_status().value;
 		_filter_control_status_changes++;
 	}
-
 	// filter fault status
 	if (_ekf.fault_status().value != _filter_fault_status) {
 		update = true;
 		_filter_fault_status = _ekf.fault_status().value;
 		_filter_fault_status_changes++;
 	}
-
 	// innovation check fail status
 	if (_ekf.innov_check_fail_status().value != _innov_check_fail_status) {
 		update = true;
 		_innov_check_fail_status = _ekf.innov_check_fail_status().value;
 		_innov_check_fail_status_changes++;
 	}
-
 	if (update) {
 		estimator_status_flags_s status_flags{};
 		status_flags.timestamp_sample = timestamp;
-
 		status_flags.control_status_changes   = _filter_control_status_changes;
 		status_flags.cs_tilt_align            = _ekf.control_status_flags().tilt_align;
 		status_flags.cs_yaw_align             = _ekf.control_status_flags().yaw_align;
@@ -1139,7 +952,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
 		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
 		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
-
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
 		status_flags.fs_bad_mag_y             = _ekf.fault_status_flags().bad_mag_y;
@@ -1159,7 +971,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.fs_bad_acc_bias          = _ekf.fault_status_flags().bad_acc_bias;
 		status_flags.fs_bad_acc_vertical      = _ekf.fault_status_flags().bad_acc_vertical;
 		status_flags.fs_bad_acc_clipping      = _ekf.fault_status_flags().bad_acc_clipping;
-
 		status_flags.innovation_fault_status_changes = _innov_check_fail_status_changes;
 		status_flags.reject_hor_vel                  = _ekf.innov_check_fail_status_flags().reject_hor_vel;
 		status_flags.reject_ver_vel                  = _ekf.innov_check_fail_status_flags().reject_ver_vel;
@@ -1174,98 +985,76 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.reject_hagl                     = _ekf.innov_check_fail_status_flags().reject_hagl;
 		status_flags.reject_optflow_x                = _ekf.innov_check_fail_status_flags().reject_optflow_X;
 		status_flags.reject_optflow_y                = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
-
 		status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 		_estimator_status_flags_pub.publish(status_flags);
-
 		_last_status_flag_update = status_flags.timestamp;
 	}
 }
-
 void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 {
 	static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
 		      "yaw_estimator_status_s::yaw wrong size");
-
 	yaw_estimator_status_s yaw_est_test_data;
-
 	if (_ekf.getDataEKFGSF(&yaw_est_test_data.yaw_composite, &yaw_est_test_data.yaw_variance,
 			       yaw_est_test_data.yaw,
 			       yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve,
 			       yaw_est_test_data.weight)) {
-
 		yaw_est_test_data.timestamp_sample = timestamp;
 		yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-
 		_yaw_est_pub.publish(yaw_est_test_data);
 	}
 }
-
 void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 {
 	if (_ekf.get_wind_status()) {
 		// Publish wind estimate only if ekf declares them valid
 		wind_s wind{};
 		wind.timestamp_sample = timestamp;
-
 		const Vector2f wind_vel = _ekf.getWindVelocity();
 		const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
 		_ekf.getAirspeedInnov(wind.tas_innov);
 		_ekf.getAirspeedInnovVar(wind.tas_innov_var);
 		_ekf.getBetaInnov(wind.beta_innov);
 		_ekf.getBetaInnovVar(wind.beta_innov_var);
-
 		wind.windspeed_north = wind_vel(0);
 		wind.windspeed_east = wind_vel(1);
 		wind.variance_north = wind_vel_var(0);
 		wind.variance_east = wind_vel_var(1);
 		wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-
 		_wind_pub.publish(wind);
 	}
 }
-
 void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &flow_sample)
 {
 	estimator_optical_flow_vel_s flow_vel{};
 	flow_vel.timestamp_sample = flow_sample.timestamp;
-
 	_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 	_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
 	_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
 	_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
 	_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
 	flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-
 	_estimator_optical_flow_vel_pub.publish(flow_vel);
 }
-
 float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
 {
 	float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
-
 	if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {
-
 		_wgs84_hgt_offset = height_diff;
 		_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
-
 	} else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {
-
 		// apply a 10 second first order low pass filter to baro offset
 		float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
 		_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
 		float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
 		_wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
 	}
-
 	return amsl_hgt + _wgs84_hgt_offset;
 }
-
 void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF airspeed sample
 	airspeed_s airspeed;
-
 	if (_airspeed_sub.update(&airspeed)) {
 		// The airspeed measurement received via the airspeed.msg topic has not been corrected
 		// for scale favtor errors and requires the ASPD_SCALE correction to be applied.
@@ -1273,10 +1062,8 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 		// was used instead, however this would introduce a potential circular dependency
 		// via the wind estimator that uses EKF velocity estimates.
 		const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
-
 		// only set airspeed data if condition for airspeed fusion are met
 		if ((_param_ekf2_arsp_thr.get() > FLT_EPSILON) && (true_airspeed_m_s > _param_ekf2_arsp_thr.get())) {
-
 			airspeedSample airspeed_sample {
 				.time_us = airspeed.timestamp,
 				.true_airspeed = true_airspeed_m_s,
@@ -1284,26 +1071,21 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 			};
 			_ekf.setAirspeedData(airspeed_sample);
 		}
-
 		ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
-
 void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF auxillary velocity sample
 	//  - use the landing target pose estimate as another source of velocity data
 	const unsigned last_generation = _landing_target_pose_sub.get_last_generation();
 	landing_target_pose_s landing_target_pose;
-
 	if (_landing_target_pose_sub.update(&landing_target_pose)) {
-
 		if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) {
 			PX4_ERR("%d - landing_target_pose lost, generation %d -> %d", _instance, last_generation,
 				_landing_target_pose_sub.get_last_generation());
 		}
-
 		// we can only use the landing target if it has a fixed position and  a valid velocity estimate
 		if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
 			// velocity of vehicle relative to target has opposite sign to target relative to vehicle
@@ -1316,59 +1098,43 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 	}
 }
-
 void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF baro sample
 	vehicle_air_data_s airdata;
-
 	if (_airdata_sub.update(&airdata)) {
 		_ekf.set_air_density(airdata.rho);
-
 		_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});
-
 		_device_id_baro = airdata.baro_device_id;
-
 		ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
-
 bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
 {
 	bool new_ev_odom = false;
 	const unsigned last_generation = _ev_odom_sub.get_last_generation();
-
 	// EKF external vision sample
 	if (_ev_odom_sub.update(&ev_odom)) {
-
 		if (_ev_odom_sub.get_last_generation() != last_generation + 1) {
 			PX4_ERR("%d - vehicle_visual_odometry lost, generation %d -> %d", _instance, last_generation,
 				_ev_odom_sub.get_last_generation());
 		}
-
 		if (_param_ekf2_aid_mask.get() & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) {
-
 			extVisionSample ev_data{};
-
 			// if error estimates are unavailable, use parameter defined defaults
-
 			// check for valid velocity data
 			if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
 				ev_data.vel(0) = ev_odom.vx;
 				ev_data.vel(1) = ev_odom.vy;
 				ev_data.vel(2) = ev_odom.vz;
-
 				if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
 					ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
-
 				} else {
 					ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
 				}
-
 				// velocity measurement error from ev_data or parameters
 				float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
-
 				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
 				    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
 				    && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
@@ -1378,20 +1144,16 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 					ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
 					ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
 					ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
-
 				} else {
 					ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
 				}
 			}
-
 			// check for valid position data
 			if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
 				ev_data.pos(0) = ev_odom.x;
 				ev_data.pos(1) = ev_odom.y;
 				ev_data.pos(2) = ev_odom.z;
-
 				float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
-
 				// position measurement error from ev_data or parameters
 				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
 				    && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
@@ -1399,55 +1161,41 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo
 					ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
 					ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
 					ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
-
 				} else {
 					ev_data.posVar.setAll(param_evp_noise_var);
 				}
 			}
-
 			// check for valid orientation data
 			if (PX4_ISFINITE(ev_odom.q[0])) {
 				ev_data.quat = Quatf(ev_odom.q);
-
 				// orientation measurement error from ev_data or parameters
 				float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
-
 				if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
 					ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
-
 				} else {
 					ev_data.angVar = param_eva_noise_var;
 				}
 			}
-
 			// use timestamp from external computer, clocks are synchronized when using MAVROS
 			ev_data.time_us = ev_odom.timestamp_sample;
 			_ekf.setExtVisionData(ev_data);
-
 			new_ev_odom = true;
 		}
-
 		ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
-
 	return new_ev_odom;
 }
-
 bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &optical_flow)
 {
 	bool new_optical_flow = false;
 	const unsigned last_generation = _optical_flow_sub.get_last_generation();
-
 	if (_optical_flow_sub.update(&optical_flow)) {
-
 		if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
 			PX4_ERR("%d - optical_flow lost, generation %d -> %d", _instance, last_generation,
 				_optical_flow_sub.get_last_generation());
 		}
-
 		if (_param_ekf2_aid_mask.get() & MASK_USE_OF) {
-
 			flowSample flow {
 				.time_us = optical_flow.timestamp,
 				// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
@@ -1457,34 +1205,26 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps, optical_flow_s &
 				.dt = 1e-6f * (float)optical_flow.integration_timespan,
 				.quality = optical_flow.quality,
 			};
-
 			if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
 			    PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
 			    flow.dt < 1) {
-
 				// Save sensor limits reported by the optical flow sensor
 				_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
 							     optical_flow.max_ground_distance);
-
 				_ekf.setOpticalFlowData(flow);
-
 				new_optical_flow = true;
 			}
 		}
-
 		ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
-
 	return new_optical_flow;
 }
-
 void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	// EKF GPS message
 	if (_param_ekf2_aid_mask.get() & MASK_USE_GPS) {
 		vehicle_gps_position_s vehicle_gps_position;
-
 		if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
 			gps_message gps_msg{
 				.time_usec = vehicle_gps_position.timestamp,
@@ -1518,7 +1258,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 					calculate_inclination_target();
 					print_target_gps();
 					//print_current_gps(gps_msg);
-					calculate_inclination_current(gps_msg);
+					check_inclination(calculate_inclination_current(gps_msg));
 				}
 			}
 			_gps_time_usec = gps_msg.time_usec;
@@ -1526,6 +1266,19 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 	}
 }
+void EKF2::check_inclination(double check)
+{
+	double origin=inclination[int(mission.current_seq)-1];
+	std:: cout << "origin : "<<origin<<"\tcheck : "<<check<<std::endl;
+	if(origin-1<=check && check <= origin+1)
+	{
+		std::cout<<"*----Good moving----*"<<std::endl;
+	}
+	else
+	{
+		std::cout<<"*----Bad moving----*"<<std::endl;
+	}
+}
 void EKF2::print_current_gps(gps_message gps_msg)
 {
 	std::cout<<"curre\t#"<<mission.current_seq<<"(lat/lon/alt)\t" << gps_msg.lat << "\t"<<gps_msg.lon<<"\t"<<gps_msg.alt<<std::endl;
@@ -1534,7 +1287,7 @@ void EKF2::calculate_inclination_target()
 {
 	double x,y;
 	inclination=new double[int(mission.count)];
-	for (size_t i = 0; i < mission.count-1; i++) {
+	for (int i = 0; i < int(mission.count)-1; i++) {
 		struct mission_item_s mission_item {};
 		struct mission_item_s next_mission_item {};
 		dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
@@ -1557,7 +1310,7 @@ void EKF2::calculate_inclination_target()
 		}*/
 		}
 }
-void EKF2::calculate_inclination_current(gps_message gps_msg)
+double EKF2::calculate_inclination_current(gps_message gps_msg)
 {
 	double x,y;
 	double check_inclination=0;
@@ -1568,13 +1321,12 @@ void EKF2::calculate_inclination_current(gps_message gps_msg)
 	}
 	std::cout.setf(std::ios::fixed);
 	std::cout.precision(7);
-	std::cout << "[Mission] #"<<mission.current_seq<<"\t"<<check_inclination<<std::endl;
+	//std::cout << "[Mission] #"<<mission.current_seq<<"\t"<<check_inclination<<std::endl;
-
+	return check_inclination;
 }
 void EKF2::print_target_gps()
 {
 	std::cout.unsetf(std::ios::fixed);
-
 	for (size_t i = 0; i < mission.count; i++) {
 		struct mission_item_s mission_item {};
 		if(int(mission.current_seq)==int(i)){
@@ -1590,63 +1342,49 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	const unsigned last_generation = _magnetometer_sub.get_last_generation();
 	vehicle_magnetometer_s magnetometer;
-
 	if (_magnetometer_sub.update(&magnetometer)) {
-
 		if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
 			perf_count(_mag_missed_perf);
 			PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
 				  _magnetometer_sub.get_last_generation());
 		}
-
 		bool reset = false;
-
 		// check if magnetometer has changed
 		if (magnetometer.device_id != _device_id_mag) {
 			if (_device_id_mag != 0) {
 				PX4_WARN("%d - mag sensor ID changed %d -> %d", _instance, _device_id_mag, magnetometer.device_id);
 			}
-
 			reset = true;
-
 		} else if (magnetometer.calibration_count > _mag_calibration_count) {
 			// existing calibration has changed, reset saved mag bias
 			PX4_DEBUG("%d - mag %d calibration updated, resetting bias", _instance, _device_id_mag);
 			reset = true;
 		}
-
 		if (reset) {
 			_ekf.resetMagBias();
 			_device_id_mag = magnetometer.device_id;
 			_mag_calibration_count = magnetometer.calibration_count;
-
 			// reset magnetometer bias learning
 			_mag_cal_total_time_us = 0;
 			_mag_cal_last_us = 0;
 			_mag_cal_available = false;
 		}
-
 		_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
-
 		ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
 	}
 }
-
 void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	if (!_distance_sensor_selected) {
 		// get subscription index of first downward-facing range sensor
 		uORB::SubscriptionMultiArray<distance_sensor_s> distance_sensor_subs{ORB_ID::distance_sensor};
-
 		for (unsigned i = 0; i < distance_sensor_subs.size(); i++) {
 			distance_sensor_s distance_sensor;
-
 			if (distance_sensor_subs[i].copy(&distance_sensor)) {
 				// only use the first instace which has the correct orientation
 				if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
 				    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
-
 					if (_distance_sensor_sub.ChangeInstance(i)) {
 						PX4_INFO("%d - selected distance_sensor:%d", _instance, i);
 						_distance_sensor_selected = true;
@@ -1655,21 +1393,16 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 			}
 		}
 	}
-
 	// EKF range sample
 	const unsigned last_generation = _distance_sensor_sub.get_last_generation();
 	distance_sensor_s distance_sensor;
-
 	if (_distance_sensor_sub.update(&distance_sensor)) {
-
 		if (_distance_sensor_sub.get_last_generation() != last_generation + 1) {
 			PX4_ERR("%d - distance_sensor lost, generation %d -> %d", _instance, last_generation,
 				_distance_sensor_sub.get_last_generation());
 		}
-
 		ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
 				(int64_t)ekf2_timestamps.timestamp / 100);
-
 		if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
 			rangeSample range_sample {
 				.time_us = distance_sensor.timestamp,
@@ -1677,29 +1410,23 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 				.quality = distance_sensor.signal_quality,
 			};
 			_ekf.setRangeData(range_sample);
-
 			// Save sensor limits reported by the rangefinder
 			_ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
-
 			_last_range_sensor_update = distance_sensor.timestamp;
 			return;
 		}
 	}
-
 	if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
 		_distance_sensor_selected = false;
 	}
 }
-
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
 	// Check if conditions are OK for learning of magnetometer bias values
 	// the EKF is operating in the correct mode and there are no filter faults
 	if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) {
-
 		if (_mag_cal_last_us != 0) {
 			_mag_cal_total_time_us += timestamp - _mag_cal_last_us;
-
 			// Start checking mag bias estimates when we have accumulated sufficient calibration time
 			if (_mag_cal_total_time_us > 30_s) {
 				_mag_cal_last_bias = _ekf.getMagBias();
@@ -1707,30 +1434,24 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 				_mag_cal_available = true;
 			}
 		}
-
 		_mag_cal_last_us = timestamp;
-
 	} else {
 		// conditions are NOT OK for learning magnetometer bias, reset timestamp
 		// but keep the accumulated calibration time
 		_mag_cal_last_us = 0;
-
 		if (_ekf.fault_status().value != 0) {
 			// if a filter fault has occurred, assume previous learning was invalid and do not
 			// count it towards total learning time.
 			_mag_cal_total_time_us = 0;
 		}
 	}
-
 	if (!_armed) {
 		// update stored declination value
 		if (!_mag_decl_saved) {
 			float declination_deg;
-
 			if (_ekf.get_mag_decl_deg(&declination_deg)) {
 				_param_ekf2_mag_decl.set(declination_deg);
 				_mag_decl_saved = true;
-
 				if (!_multi_mode) {
 					_param_ekf2_mag_decl.commit_no_notification();
 				}
@@ -1738,50 +1459,39 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 		}
 	}
 }
-
 int EKF2::custom_command(int argc, char *argv[])
 {
 	return print_usage("unknown command");
 }
-
 int EKF2::task_spawn(int argc, char *argv[])
 {
 	bool success = false;
 	bool replay_mode = false;
-
 	if (argc > 1 && !strcmp(argv[1], "-r")) {
 		PX4_INFO("replay mode enabled");
 		replay_mode = true;
 	}
-
 #if !defined(CONSTRAINED_FLASH)
 	bool multi_mode = false;
 	int32_t imu_instances = 0;
 	int32_t mag_instances = 0;
-
 	int32_t sens_imu_mode = 1;
 	param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode);
-
 	if (sens_imu_mode == 0) {
 		// ekf selector requires SENS_IMU_MODE = 0
 		multi_mode = true;
-
 		// IMUs (1 - 4 supported)
 		param_get(param_find("EKF2_MULTI_IMU"), &imu_instances);
-
 		if (imu_instances < 1 || imu_instances > 4) {
 			const int32_t imu_instances_limited = math::constrain(imu_instances, 1, 4);
 			PX4_WARN("EKF2_MULTI_IMU limited %d -> %d", imu_instances, imu_instances_limited);
 			param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited);
 			imu_instances = imu_instances_limited;
 		}
-
 		int32_t sens_mag_mode = 1;
 		param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode);
-
 		if (sens_mag_mode == 0) {
 			param_get(param_find("EKF2_MULTI_MAG"), &mag_instances);
-
 			// Mags (1 - 4 supported)
 			if (mag_instances < 1 || mag_instances > 4) {
 				const int32_t mag_instances_limited = math::constrain(mag_instances, 1, 4);
@@ -1789,167 +1499,126 @@ int EKF2::task_spawn(int argc, char *argv[])
 				param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited);
 				mag_instances = mag_instances_limited;
 			}
-
 		} else {
 			mag_instances = 1;
 		}
 	}
-
 	if (multi_mode) {
 		// Start EKF2Selector if it's not already running
 		if (_ekf2_selector.load() == nullptr) {
 			EKF2Selector *inst = new EKF2Selector();
-
 			if (inst) {
 				_ekf2_selector.store(inst);
-
 			} else {
 				PX4_ERR("Failed to create EKF2 selector");
 				return PX4_ERROR;
 			}
 		}
-
 		const hrt_abstime time_started = hrt_absolute_time();
 		const int multi_instances = math::min(imu_instances * mag_instances, (int)EKF2_MAX_INSTANCES);
 		int multi_instances_allocated = 0;
-
 		// allocate EKF2 instances until all found or arming
 		uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
-
 		bool ekf2_instance_created[4][4] {}; // IMUs * mags
-
 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
 		       && ((hrt_elapsed_time(&time_started) < 30_s)
 			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
-
 			vehicle_status_sub.update();
-
 			for (uint8_t mag = 0; mag < mag_instances; mag++) {
 				uORB::SubscriptionData<vehicle_magnetometer_s> vehicle_mag_sub{ORB_ID(vehicle_magnetometer), mag};
-
 				for (uint8_t imu = 0; imu < imu_instances; imu++) {
-
 					uORB::SubscriptionData<vehicle_imu_s> vehicle_imu_sub{ORB_ID(vehicle_imu), imu};
 					vehicle_mag_sub.update();
-
 					// Mag & IMU data must be valid, first mag can be ignored initially
 					if ((vehicle_mag_sub.get().device_id != 0 || mag == 0)
 					    && (vehicle_imu_sub.get().accel_device_id != 0)
 					    && (vehicle_imu_sub.get().gyro_device_id != 0)) {
-
 						if (!ekf2_instance_created[imu][mag]) {
 							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
-
 							if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
 								int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
-
 								if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
 									_objects[actual_instance].store(ekf2_inst);
 									success = true;
 									multi_instances_allocated++;
 									ekf2_instance_created[imu][mag] = true;
-
 									if (actual_instance == 0) {
 										// force selector to run immediately if first instance started
 										_ekf2_selector.load()->ScheduleNow();
 									}
-
 									PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance,
 										 imu, vehicle_imu_sub.get().accel_device_id,
 										 mag, vehicle_mag_sub.get().device_id);
-
 									// sleep briefly before starting more instances
 									px4_usleep(10000);
-
 								} else {
 									PX4_ERR("instance numbering problem instance: %d", actual_instance);
 									delete ekf2_inst;
 									break;
 								}
-
 							} else {
 								PX4_ERR("alloc and init failed imu: %d mag:%d", imu, mag);
 								px4_usleep(1000000);
 								break;
 							}
 						}
-
 					} else {
 						px4_usleep(50000); // give the sensors extra time to start
 						continue;
 					}
 				}
 			}
-
 			if (multi_instances_allocated < multi_instances) {
 				px4_usleep(100000);
 			}
 		}
-
 	}
-
 #endif // !CONSTRAINED_FLASH
-
 	else {
 		// otherwise launch regular
 		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
-
 		if (ekf2_inst) {
 			_objects[0].store(ekf2_inst);
 			ekf2_inst->ScheduleNow();
 			success = true;
 		}
 	}
-
 	return success ? PX4_OK : PX4_ERROR;
 }
-
 int EKF2::print_usage(const char *reason)
 {
 	if (reason) {
 		PX4_WARN("%s\n", reason);
 	}
-
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
 Attitude and position estimator using an Extended Kalman Filter. It is used for Multirotors and Fixed-Wing.
-
 The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html) page.
-
 ekf2 can be started in replay mode (`-r`): in this mode it does not access the system time, but only uses the
 timestamps from the sensor topics.
-
 )DESCR_STR");
-
 	PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
 	return 0;
 }
-
 extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 {
 	if (argc <= 1 || strcmp(argv[1], "-h") == 0) {
 		return EKF2::print_usage();
 	}
-
 	if (strcmp(argv[1], "start") == 0) {
 		int ret = 0;
 		EKF2::lock_module();
-
 		ret = EKF2::task_spawn(argc - 1, argv + 1);
-
 		if (ret < 0) {
 			PX4_ERR("start failed (%i)", ret);
 		}
-
 		EKF2::unlock_module();
 		return ret;
-
 	} else if (strcmp(argv[1], "status") == 0) {
 		if (EKF2::trylock_module()) {
 #if !defined(CONSTRAINED_FLASH)
@@ -1957,32 +1626,24 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 				_ekf2_selector.load()->PrintStatus();
 			}
 #endif // !CONSTRAINED_FLASH
-
 			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
 				if (_objects[i].load()) {
 					PX4_INFO_RAW("\n");
 					_objects[i].load()->print_status();
 				}
 			}
-
 			EKF2::unlock_module();
-
 		} else {
 			PX4_WARN("module locked, try again later");
 		}
-
 		return 0;
-
 	} else if (strcmp(argv[1], "stop") == 0) {
 		EKF2::lock_module();
-
 		if (argc > 2) {
 			int instance = atoi(argv[2]);
-
 			if (instance >= 0 && instance < EKF2_MAX_INSTANCES) {
 				PX4_INFO("stopping instance %d", instance);
 				EKF2 *inst = _objects[instance].load();
-
 				if (inst) {
 					inst->request_stop();
 					px4_usleep(20000); // 20 ms
@@ -1992,11 +1653,9 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 			} else {
 				PX4_ERR("invalid instance %d", instance);
 			}
-
 		} else {
 			// otherwise stop everything
 			bool was_running = false;
-
 #if !defined(CONSTRAINED_FLASH)
 			if (_ekf2_selector.load()) {
 				PX4_INFO("stopping ekf2 selector");
@@ -2006,10 +1665,8 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 				was_running = true;
 			}
 #endif // !CONSTRAINED_FLASH
-
 			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
 				EKF2 *inst = _objects[i].load();
-
 				if (inst) {
 					PX4_INFO("stopping ekf2 instance %d", i);
 					was_running = true;
@@ -2019,19 +1676,15 @@ extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
 					_objects[i].store(nullptr);
 				}
 			}
-
 			if (!was_running) {
 				PX4_WARN("not running");
 			}
 		}
-
 		EKF2::unlock_module();
 		return PX4_OK;
 	}
-
 	EKF2::lock_module(); // Lock here, as the method could access _object.
 	int ret = EKF2::custom_command(argc - 1, argv + 1);
 	EKF2::unlock_module();
-
 	return ret;
 }
--- a/home/Firmware/src/modules/ekf2/EKF2.hpp
View file @f8ff5eb
+++ b/home/Firmware/src/modules/ekf2/EKF2.hpp
View file @f8ff5eb
@@ -30,18 +30,14 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
 /**
  * @file EKF2.cpp
  * Implementation of the attitude and position estimator.
  *
  * @author Roman Bapst
  */
-
 #pragma once
-
 #include "EKF2Selector.hpp"
-
 #include <float.h>
 #include <dataman/dataman.h>
 #include <containers/LockGuard.hpp>
@@ -89,44 +85,30 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <uORB/topics/yaw_estimator_status.h>
-
 #include "Utility/PreFlightChecker.hpp"
-
 extern pthread_mutex_t ekf2_module_mutex;
-
 class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	EKF2() = delete;
 	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode);
 	~EKF2() override;
-
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
-
 	/** @see ModuleBase */
 	static int custom_command(int argc, char *argv[]);
-
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
-
 	int print_status();
-
 	bool should_exit() const { return _task_should_exit.load(); }
-
 	void request_stop() { _task_should_exit.store(true); }
-
 	static void lock_module() { pthread_mutex_lock(&ekf2_module_mutex); }
 	static bool trylock_module() { return (pthread_mutex_trylock(&ekf2_module_mutex) == 0); }
 	static void unlock_module() { pthread_mutex_unlock(&ekf2_module_mutex); }
-
 	bool multi_init(int imu, int mag);
-
 	int instance() const { return _instance; }
-
 private:
 	void Run() override;
-
 	void PublishAttitude(const hrt_abstime &timestamp);
 	void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
 	void PublishEventFlags(const hrt_abstime &timestamp);
@@ -144,7 +126,6 @@ private:
 	void PublishStatusFlags(const hrt_abstime &timestamp);
 	void PublishWindEstimate(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
-
 	void UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps);
@@ -153,12 +134,12 @@ private:
 	void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
-
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
 	void print_target_gps();
 	void print_current_gps(gps_message gps_msg);
 	void calculate_inclination_target();
-	void calculate_inclination_current(gps_message gps_msg);
+	double calculate_inclination_current(gps_message gps_msg);
+	void check_inclination(double check);
 	double* inclination=nullptr;
 	int32_t target_lat,target_lon,target_alt;
 	mission_s mission;
@@ -166,60 +147,44 @@ private:
 	 * Calculate filtered WGS84 height from estimated AMSL height
 	 */
 	float filter_altitude_ellipsoid(float amsl_hgt);
-
 	static constexpr float sq(float x) { return x * x; };
 	const bool _replay_mode{false};			///< true when we use replay data from a log
 	const bool _multi_mode;
 	int _instance{0};
-
 	px4::atomic_bool _task_should_exit{false};
-
 	// time slip monitoring
 	uint64_t _integrated_time_us = 0;	///< integral of gyro delta time from start (uSec)
 	uint64_t _start_time_us = 0;		///< system time at EKF start (uSec)
 	int64_t _last_time_slip_us = 0;		///< Last time slip (uSec)
-
 	perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
 	perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
 	perf_counter_t _imu_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
 	perf_counter_t _mag_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": mag message missed")};
-
 	// Used to check, save and use learned magnetometer biases
 	hrt_abstime _mag_cal_last_us{0};	///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
 	hrt_abstime _mag_cal_total_time_us{0};	///< accumulated calibration time since the last save
-
 	Vector3f _mag_cal_last_bias{};	///< last valid XYZ magnetometer bias estimates (Gauss)
 	Vector3f _mag_cal_last_bias_variance{};	///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2)
 	bool _mag_cal_available{false};	///< true when an unsaved valid calibration for the XYZ magnetometer bias is available
-
 	// Used to control saving of mag declination to be used on next startup
 	bool _mag_decl_saved = false;	///< true when the magnetic declination has been saved
-
 	bool _had_valid_terrain{false};			///< true if at any time there was a valid terrain estimate
-
 	uint64_t _gps_time_usec{0};
 	int32_t _gps_alttitude_ellipsoid{0};			///< altitude in 1E-3 meters (millimeters) above ellipsoid
 	uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt
 	float   _wgs84_hgt_offset = 0;  ///< height offset between AMSL and WGS84
-
 	uint8_t _imu_calibration_count{0};
 	uint8_t _mag_calibration_count{0};
-
 	uint32_t _device_id_accel{0};
 	uint32_t _device_id_baro{0};
 	uint32_t _device_id_gyro{0};
 	uint32_t _device_id_mag{0};
-
 	Vector3f _last_local_position_for_gpos{};
-
 	Vector3f _last_accel_bias_published{};
 	Vector3f _last_gyro_bias_published{};
 	Vector3f _last_mag_bias_published{};
-
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
-
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
 	uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)};
@@ -232,29 +197,22 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
-
 	bool _callback_registered{false};
-
 	bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations
 	bool _armed{false};
 	bool _standby{false}; // standby arming state
-
 	hrt_abstime _last_status_flag_update{0};
 	hrt_abstime _last_range_sensor_update{0};
-
 	uint32_t _filter_control_status{0};
 	uint32_t _filter_fault_status{0};
 	uint32_t _innov_check_fail_status{0};
-
 	uint32_t _filter_control_status_changes{0};
 	uint32_t _filter_fault_status_changes{0};
 	uint32_t _innov_check_fail_status_changes{0};
 	uint32_t _filter_warning_event_changes{0};
 	uint32_t _filter_information_event_changes{0};
-
 	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::PublicationMulti<ekf_gps_drift_s>              _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
@@ -268,21 +226,15 @@ private:
 	uORB::PublicationMulti<estimator_event_flags_s>      _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
 	uORB::PublicationMulti<vehicle_odometry_s>           _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
 	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
-
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;
 	uORB::PublicationMulti<vehicle_global_position_s>    _global_position_pub;
 	uORB::PublicationMulti<vehicle_odometry_s>           _odometry_pub;
 	uORB::PublicationMulti<wind_s>              _wind_pub;
-
-
 	PreFlightChecker _preflt_checker;
-
 	Ekf _ekf;
-
 	parameters *_params;	///< pointer to ekf parameter struct (located in _ekf class instance)
-
 	DEFINE_PARAMETERS(
 		(ParamExtInt<px4::params::EKF2_MIN_OBS_DT>)
 		_param_ekf2_min_obs_dt,	///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec)
@@ -302,12 +254,10 @@ private:
 		_param_ekf2_ev_delay,	///< off-board vision measurement delay relative to the IMU (mSec)
 		(ParamExtFloat<px4::params::EKF2_AVEL_DELAY>)
 		_param_ekf2_avel_delay,	///< auxillary velocity measurement delay relative to the IMU (mSec)
-
 		(ParamExtFloat<px4::params::EKF2_GYR_NOISE>)
 		_param_ekf2_gyr_noise,	///< IMU angular rate noise used for covariance prediction (rad/sec)
 		(ParamExtFloat<px4::params::EKF2_ACC_NOISE>)
 		_param_ekf2_acc_noise,	///< IMU acceleration noise use for covariance prediction (m/sec**2)
-
 		// process noise
 		(ParamExtFloat<px4::params::EKF2_GYR_B_NOISE>)
 		_param_ekf2_gyr_b_noise,	///< process noise for IMU rate gyro bias prediction (rad/sec**2)
@@ -322,7 +272,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise,	///< process noise for terrain offset (m/sec)
 		(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
 		_param_ekf2_terr_grad,	///< gradient of terrain used to estimate process noise due to changing position (m/m)
-
 		(ParamExtFloat<px4::params::EKF2_GPS_V_NOISE>)
 		_param_ekf2_gps_v_noise,	///< minimum allowed observation noise for gps velocity fusion (m/sec)
 		(ParamExtFloat<px4::params::EKF2_GPS_P_NOISE>)
@@ -343,7 +292,6 @@ private:
 		_param_ekf2_gps_v_gate,	///< GPS velocity innovation consistency gate size (STD)
 		(ParamExtFloat<px4::params::EKF2_TAS_GATE>)
 		_param_ekf2_tas_gate,	///< True Airspeed innovation consistency gate size (STD)
-
 		// control of magnetometer fusion
 		(ParamExtFloat<px4::params::EKF2_HEAD_NOISE>)
 		_param_ekf2_head_noise,	///< measurement noise used for simple heading fusion (rad)
@@ -367,7 +315,6 @@ private:
 		_param_ekf2_mag_acclim,	///< integer used to specify the type of magnetometer fusion used
 		(ParamExtFloat<px4::params::EKF2_MAG_YAWLIM>)
 		_param_ekf2_mag_yawlim,	///< yaw rate threshold used by mode select logic (rad/sec)
-
 		(ParamExtInt<px4::params::EKF2_GPS_CHECK>)
 		_param_ekf2_gps_check,	///< bitmask used to control which GPS quality checks are used
 		(ParamExtFloat<px4::params::EKF2_REQ_EPH>) _param_ekf2_req_eph,	///< maximum acceptable horiz position error (m)
@@ -379,7 +326,6 @@ private:
 		(ParamExtFloat<px4::params::EKF2_REQ_HDRIFT>)
 		_param_ekf2_req_hdrift,	///< maximum acceptable horizontal drift speed (m/s)
 		(ParamExtFloat<px4::params::EKF2_REQ_VDRIFT>) _param_ekf2_req_vdrift,	///< maximum acceptable vertical drift speed (m/s)
-
 		// measurement source control
 		(ParamExtInt<px4::params::EKF2_AID_MASK>)
 		_param_ekf2_aid_mask,		///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
@@ -388,7 +334,6 @@ private:
 		_param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation
 		(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
 		_param_ekf2_noaid_tout,	///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
-
 		// range finder fusion
 		(ParamExtFloat<px4::params::EKF2_RNG_NOISE>)
 		_param_ekf2_rng_noise,	///< observation noise for range finder measurements (m)
@@ -407,7 +352,6 @@ private:
 		_param_ekf2_rng_a_igate,	///< gate size used for innovation consistency checks for range aid fusion (STD)
 		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>)
 		_param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
-
 		// vision estimate fusion
 		(ParamInt<px4::params::EKF2_EV_NOISE_MD>)
 		_param_ekf2_ev_noise_md,	///< determine source of vision observation noise
@@ -421,7 +365,6 @@ private:
 		_param_ekf2_evv_gate,	///< external vision velocity innovation consistency gate size (STD)
 		(ParamExtFloat<px4::params::EKF2_EVP_GATE>)
 		_param_ekf2_evp_gate,	///< external vision position innovation consistency gate size (STD)
-
 		// optical flow fusion
 		(ParamExtFloat<px4::params::EKF2_OF_N_MIN>)
 		_param_ekf2_of_n_min,	///< best quality observation noise for optical flow LOS rate measurements (rad/sec)
@@ -431,7 +374,6 @@ private:
 		_param_ekf2_of_qmin,	///< minimum acceptable quality integer from  the flow sensor
 		(ParamExtFloat<px4::params::EKF2_OF_GATE>)
 		_param_ekf2_of_gate,	///< optical flow fusion innovation consistency gate size (STD)
-
 		// sensor positions in body frame
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_X>) _param_ekf2_imu_pos_x,		///< X position of IMU in body frame (m)
 		(ParamExtFloat<px4::params::EKF2_IMU_POS_Y>) _param_ekf2_imu_pos_y,		///< Y position of IMU in body frame (m)
@@ -454,19 +396,16 @@ private:
 		_param_ekf2_ev_pos_y,		///< Y position of VI sensor focal point in body frame (m)
 		(ParamExtFloat<px4::params::EKF2_EV_POS_Z>)
 		_param_ekf2_ev_pos_z,		///< Z position of VI sensor focal point in body frame (m)
-
 		// control of airspeed and sideslip fusion
 		(ParamFloat<px4::params::EKF2_ARSP_THR>)
 		_param_ekf2_arsp_thr, 	///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
 		(ParamInt<px4::params::EKF2_FUSE_BETA>)
 		_param_ekf2_fuse_beta,		///< Controls synthetic sideslip fusion, 0 disables, 1 enables
-
 		// output predictor filter time constants
 		(ParamExtFloat<px4::params::EKF2_TAU_VEL>)
 		_param_ekf2_tau_vel,		///< time constant used by the output velocity complementary filter (sec)
 		(ParamExtFloat<px4::params::EKF2_TAU_POS>)
 		_param_ekf2_tau_pos,		///< time constant used by the output position complementary filter (sec)
-
 		// IMU switch on bias parameters
 		(ParamExtFloat<px4::params::EKF2_GBIAS_INIT>)
 		_param_ekf2_gbias_init,	///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
@@ -474,7 +413,6 @@ private:
 		_param_ekf2_abias_init,	///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
 		(ParamExtFloat<px4::params::EKF2_ANGERR_INIT>)
 		_param_ekf2_angerr_init,	///< 1-sigma tilt error after initial alignment using gravity vector (rad)
-
 		// EKF accel bias learning control
 		(ParamExtFloat<px4::params::EKF2_ABL_LIM>) _param_ekf2_abl_lim,	///< Accelerometer bias learning limit (m/s**2)
 		(ParamExtFloat<px4::params::EKF2_ABL_ACCLIM>)
@@ -483,13 +421,11 @@ private:
 		_param_ekf2_abl_gyrlim,	///< Maximum IMU gyro angular rate magnitude that allows IMU bias learning (m/s**2)
 		(ParamExtFloat<px4::params::EKF2_ABL_TAU>)
 		_param_ekf2_abl_tau,	///< Time constant used to inhibit IMU delta velocity bias learning (sec)
-
 		// Multi-rotor drag specific force fusion
 		(ParamExtFloat<px4::params::EKF2_DRAG_NOISE>)
 		_param_ekf2_drag_noise,	///< observation noise variance for drag specific force measurements (m/sec**2)**2
 		(ParamExtFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x,		///< ballistic coefficient along the X-axis (kg/m**2)
 		(ParamExtFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y,		///< ballistic coefficient along the Y-axis (kg/m**2)
-
 		// Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth
 		// Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2
 		(ParamExtFloat<px4::params::EKF2_ASPD_MAX>)
@@ -504,19 +440,15 @@ private:
 		_param_ekf2_pcoef_yn,	///< static pressure position error coefficient along the negative Y body axis
 		(ParamExtFloat<px4::params::EKF2_PCOEF_Z>)
 		_param_ekf2_pcoef_z,	///< static pressure position error coefficient along the Z body axis
-
 		// Test used to determine if the vehicle is static or moving
 		(ParamExtFloat<px4::params::EKF2_MOVE_TEST>)
 		_param_ekf2_move_test,	///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving.
-
 		(ParamFloat<px4::params::EKF2_REQ_GPS_H>) _param_ekf2_req_gps_h, ///< Required GPS health time
 		(ParamExtInt<px4::params::EKF2_MAG_CHECK>) _param_ekf2_mag_check, ///< Mag field strength check
 		(ParamExtInt<px4::params::EKF2_SYNT_MAG_Z>)
 		_param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone.
-
 		// Used by EKF-GSF experimental yaw estimator
 		(ParamExtFloat<px4::params::EKF2_GSF_TAS>)
 		_param_ekf2_gsf_tas_default	///< default value of true airspeed assumed during fixed wing operation
-
 	)
 };