fw_att_pos_estimator: Removed unused code

1 files changed, 1 insertions, 96 deletions

diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 38162959d..bb3541a39 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -790,6 +790,7 @@ FixedwingEstimator::task_main()
 				RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
 				// run the fusion step
 				FuseVelposNED();
+
 			} else {
 				fuseVelData = false;
 				fusePosData = false;
@@ -829,102 +830,6 @@ FixedwingEstimator::task_main()
 				fuseVtasData = false;
 			}
 
-			// if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) {
-			// 	InitialiseFilter(velNED);
-			// 	_initialized = true;
-
-			// 	warnx("init done.");
-			// }
-
-			// if (_initialized) {
-
-			// 	/* predict states and covariances */
-
-			// 	/* run the strapdown INS every sensor update */
-			// 	UpdateStrapdownEquationsNED();
-
-			// 	/* store the predictions */
-			// 	StoreStates(IMUmsec);
-
-			// 	/* evaluate if on ground */
-			// 	// OnGroundCheck();
-			// 	onGround = false;
-
-			// 	/* prepare the delta angles and time used by the covariance prediction */
-			// 	summedDelAng = summedDelAng + correctedDelAng;
-			// 	summedDelVel = summedDelVel + correctedDelVel;
-
-			// 	dt += dtIMU;
-
-			// 	/* predict the covairance if the total delta angle has exceeded the threshold
-			// 	 * or the time limit will be exceeded on the next measurement update
-			// 	 */
-			// 	if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) {
-			// 		CovariancePrediction(dt);
-			// 		summedDelAng = summedDelAng.zero();
-			// 		summedDelVel = summedDelVel.zero();
-			// 		dt = 0.0f;
-			// 	}
-
-			// }
-
-
-			// if (gps_updated && _initialized) {
-
-			// 	/* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */
-			// 	calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
-
-			// 	posNE[0] = posNED[0];
-			// 	posNE[1] = posNED[1];
-
-			// 	// set flags for further processing
-			// 	fuseVelData = true;
-			// 	fusePosData = true;
-
-			// 	/* recall states after adjusting for delays */
-			// 	RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-			// 	RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-
-			// 	/* run the actual fusion */
-			// 	FuseVelposNED();
-			// } else {
-			// 	fuseVelData = false;
-			// 	fusePosData = false;
-			// }
-
-			// if (baro_updated && _initialized) {
-
-			// 	fuseHgtData = true;
-			// 	// recall states stored at time of measurement after adjusting for delays
-			// 	RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
-			// 	// run the fusion step
-			// 	FuseVelposNED();
-			// } else {
-			// 	fuseHgtData = false;
-			// }
-
-			// if (mag_updated && _initialized) {
-			// 	fuseMagData = true;
-			// 	RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
-
-			// } else {
-			// 	fuseMagData = false;
-			// }
-
-			// if (_initialized) {
-			// 	FuseMagnetometer();
-			// }
-
-			// if (airspeed_updated && _initialized
-			// 	&& _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) {
-
-			// 	fuseVtasData = true;
-			// 	RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
-			// 	FuseAirspeed();
-			// } else {
-			// 	fuseVtasData = false;
-			// }
-
 			// Publish results
 			if (_initialized) {