Merge pull request #1159 from PX4/airspeed_test_fix

Generalized the airspeed check

2 files changed, 60 insertions, 81 deletions

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 247814a7e..40cb6043f 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -568,61 +568,26 @@ FixedwingEstimator::check_filter_state()
 
 	int check = _ekf->CheckAndBound(&ekf_report);
 
-	const char* ekfname = "att pos estimator: ";
-
-	switch (check) {
-		case 0:
-			/* all ok */
-			break;
-		case 1:
-		{
-			const char* str = "NaN in states, resetting";
-			warnx("%s", str);
-			mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
-			break;
-		}
-		case 2:
-		{
-			const char* str = "stale IMU data, resetting";
-			warnx("%s", str);
-			mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
-			break;
-		}
-		case 3:
-		{
-			const char* str = "switching to dynamic state";
-			warnx("%s", str);
-			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
-			break;
-		}
-		case 4:
-		{
-			const char* str = "excessive gyro offsets";
-			warnx("%s", str);
-			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
-			break;
-		}
-		case 5:
-		{
-			const char* str = "GPS velocity divergence";
-			warnx("%s", str);
-			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
-			break;
-		}
-		case 6:
-		{
-			const char* str = "excessive covariances";
-			warnx("%s", str);
-			mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
-			break;
+	const char* const feedback[] = { 0,
+					"NaN in states, resetting",
+					"stale IMU data, resetting",
+					"got initial position lock",
+					"excessive gyro offsets",
+					"GPS velocity divergence",
+					"excessive covariances",
+					"unknown condition"};
+
+	// Print out error condition
+	if (check) {
+		unsigned warn_index = static_cast<unsigned>(check);
+		unsigned max_warn_index = (sizeof(feedback) / sizeof(feedback[0]));
+
+		if (max_warn_index < warn_index) {
+			warn_index = max_warn_index;
 		}
 
-		default:
-		{
-			const char* str = "unknown reset condition";
-			warnx("%s", str);
-			mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
-		}
+		warnx("reset: %s", feedback[warn_index]);
+		mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
 	}
 
 	struct estimator_status_report rep;
@@ -654,6 +619,10 @@ FixedwingEstimator::check_filter_state()
 		rep.health_flags |= (((uint8_t)ekf_report.posHealth)	<< 1);
 		rep.health_flags |= (((uint8_t)ekf_report.hgtHealth)	<< 2);
 		rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive)	<< 3);
+		// rep.health_flags |= (((uint8_t)ekf_report.onGround)	<< 4);
+		// rep.health_flags |= (((uint8_t)ekf_report.staticMode)	<< 5);
+		// rep.health_flags |= (((uint8_t)ekf_report.useCompass)	<< 6);
+		// rep.health_flags |= (((uint8_t)ekf_report.useAirspeed)	<< 7);
 
 		rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout)	<< 0);
 		rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout)	<< 1);
@@ -1213,10 +1182,10 @@ FixedwingEstimator::task_main()
 					_baro_gps_offset = 0.0f;
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
+
 				} else if (_ekf->statesInitialised) {
 
 					// We're apparently initialized in this case now
-
 					int check = check_filter_state();
 
 					if (check) {
@@ -1224,7 +1193,6 @@ FixedwingEstimator::task_main()
 						continue;
 					}
 
-
 					// Run the strapdown INS equations every IMU update
 					_ekf->UpdateStrapdownEquationsNED();
 	#if 0
@@ -1292,7 +1260,11 @@ FixedwingEstimator::task_main()
 						// run the fusion step
 						_ekf->FuseVelposNED();
 
-					} else if (_ekf->statesInitialised) {
+					} else if (!_gps_initialized) {
+
+						// force static mode
+						_ekf->staticMode = true;
+
 						// Convert GPS measurements to Pos NE, hgt and Vel NED
 						_ekf->velNED[0] = 0.0f;
 						_ekf->velNED[1] = 0.0f;
@@ -1314,7 +1286,7 @@ FixedwingEstimator::task_main()
 						_ekf->fusePosData = false;
 					}
 
-					if (newHgtData && _ekf->statesInitialised) {
+					if (newHgtData) {
 						// Could use a blend of GPS and baro alt data if desired
 						_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
 						_ekf->fuseHgtData = true;
@@ -1328,7 +1300,7 @@ FixedwingEstimator::task_main()
 					}
 
 					// Fuse Magnetometer Measurements
-					if (newDataMag && _ekf->statesInitialised) {
+					if (newDataMag) {
 						_ekf->fuseMagData = true;
 						_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
@@ -1342,7 +1314,7 @@ FixedwingEstimator::task_main()
 					}
 
 					// Fuse Airspeed Measurements
-					if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
+					if (newAdsData && _ekf->VtasMeas > 7.0f) {
 						_ekf->fuseVtasData = true;
 						_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
 						_ekf->FuseAirspeed();
@@ -1410,7 +1382,7 @@ FixedwingEstimator::task_main()
 
 						_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
 						_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
-						_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s;
+						_airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
 
 
 						/* crude land detector for fixedwing only,
@@ -1501,27 +1473,28 @@ FixedwingEstimator::task_main()
 
 						}
 
-					}
-
-				}
+						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+							_wind.timestamp = _global_pos.timestamp;
+							_wind.windspeed_north = _ekf->states[14];
+							_wind.windspeed_east = _ekf->states[15];
+							_wind.covariance_north = _ekf->P[14][14];
+							_wind.covariance_east = _ekf->P[15][15];
 
-				if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
-					_wind.timestamp = _global_pos.timestamp;
-					_wind.windspeed_north = _ekf->states[14];
-					_wind.windspeed_east = _ekf->states[15];
-					_wind.covariance_north = _ekf->P[14][14];
-					_wind.covariance_east = _ekf->P[15][15];
+							/* lazily publish the wind estimate only once available */
+							if (_wind_pub > 0) {
+								/* publish the wind estimate */
+								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
 
-					/* lazily publish the wind estimate only once available */
-					if (_wind_pub > 0) {
-						/* publish the wind estimate */
-						orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
+							} else {
+								/* advertise and publish */
+								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+							}
+						}
 
-					} else {
-						/* advertise and publish */
-						_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
 					}
+
 				}
+
 			}
 
 		}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 7f9486eb5..768e0be35 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2279,7 +2279,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
 
 void AttPosEKF::OnGroundCheck()
 {
-    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f));
+    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
     if (staticMode) {
         staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
     }
@@ -2879,12 +2879,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     current_ekf_state.statesNaN = false;
 
     current_ekf_state.velHealth = true;
-    //current_ekf_state.posHealth = ?;
-    //current_ekf_state.hgtHealth = ?;
+    current_ekf_state.posHealth = true;
+    current_ekf_state.hgtHealth = true;
     
     current_ekf_state.velTimeout = false;
-    //current_ekf_state.posTimeout = ?;
-    //current_ekf_state.hgtTimeout = ?;
+    current_ekf_state.posTimeout = false;
+    current_ekf_state.hgtTimeout = false;
+
+    fuseVelData = false;
+    fusePosData = false;
+    fuseHgtData = false;
+    fuseMagData = false;
+    fuseVtasData = false;
 
     // Fill variables with valid data
     velNED[0] = initvelNED[0];