Merge branch 'terrainaltfield' of github.com:PX4/Firmware

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -58,6 +58,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_mag.h>
 #include <drivers/drv_baro.h>
+#include <drivers/drv_range_finder.h>
 #ifdef SENSOR_COMBINED_SUB
 #include <uORB/topics/sensor_combined.h>
 #endif
@@ -96,7 +97,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
 
 __EXPORT uint32_t millis();
 
+__EXPORT uint64_t getMicros();
+
 static uint64_t IMUmsec = 0;
+static uint64_t IMUusec = 0;
 static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
 
 uint32_t millis()
@@ -104,6 +108,11 @@ uint32_t millis()
 	return IMUmsec;
 }
 
+uint64_t getMicros()
+{
+	return IMUusec;
+}
+
 class FixedwingEstimator
 {
 public:
@@ -171,6 +180,7 @@ private:
 #else
 	int		_sensor_combined_sub;
 #endif
+	int		_distance_sub;			/**< distance measurement */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_baro_sub;			/**< barometer subscription */
 	int		_gps_sub;			/**< GPS subscription */
@@ -196,7 +206,8 @@ private:
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct vehicle_local_position_s			_local_pos;		/**< local vehicle position */
 	struct vehicle_gps_position_s			_gps;			/**< GPS position */
-	struct wind_estimate_s				_wind;			/**< Wind estimate */
+	struct wind_estimate_s				_wind;			/**< wind estimate */
+	struct range_finder_report			_distance;		/**< distance estimate */
 
 	struct gyro_scale				_gyro_offsets;
 	struct accel_scale				_accel_offsets;
@@ -226,6 +237,7 @@ private:
 	hrt_abstime					_filter_start_time;
 	hrt_abstime					_last_sensor_timestamp;
 	hrt_abstime					_last_run;
+	hrt_abstime					_distance_last_valid;
 	bool						_gyro_valid;
 	bool						_accel_valid;
 	bool						_mag_valid;
@@ -342,6 +354,7 @@ FixedwingEstimator::FixedwingEstimator() :
 #else
 	_sensor_combined_sub(-1),
 #endif
+	_distance_sub(-1),
 	_airspeed_sub(-1),
 	_baro_sub(-1),
 	_gps_sub(-1),
@@ -399,6 +412,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_filter_start_time(0),
 	_last_sensor_timestamp(0),
 	_last_run(0),
+	_distance_last_valid(0),
 	_gyro_valid(false),
 	_accel_valid(false),
 	_mag_valid(false),
@@ -549,6 +563,7 @@ FixedwingEstimator::parameters_update()
 		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
         	_ekf->accelProcessNoise = _parameters.acc_pnoise;
 		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
+		_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
 	}
 
 	return OK;
@@ -704,6 +719,7 @@ FixedwingEstimator::task_main()
 	/*
 	 * do subscriptions
 	 */
+	_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -753,6 +769,7 @@ FixedwingEstimator::task_main()
 	bool newHgtData = false;
 	bool newAdsData = false;
 	bool newDataMag = false;
+	bool newRangeData = false;
 
 	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
 
@@ -850,7 +867,8 @@ FixedwingEstimator::task_main()
 			}
 
 			_last_sensor_timestamp = _gyro.timestamp;
-			IMUmsec = _gyro.timestamp / 1e3f;
+			IMUmsec = _gyro.timestamp / 1e3;
+			IMUusec = _gyro.timestamp;
 
 			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
 			_last_run = _gyro.timestamp;
@@ -914,7 +932,8 @@ FixedwingEstimator::task_main()
 
 			// Copy gyro and accel
 			_last_sensor_timestamp = _sensor_combined.timestamp;
-			IMUmsec = _sensor_combined.timestamp / 1e3f;
+			IMUmsec = _sensor_combined.timestamp / 1e3;
+			IMUusec = _sensor_combined.timestamp;
 
 			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
 
@@ -994,8 +1013,6 @@ FixedwingEstimator::task_main()
 
 			if (gps_updated) {
 
-				last_gps = _gps.timestamp_position;
-
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 				perf_count(_perf_gps);
 
@@ -1008,11 +1025,17 @@ FixedwingEstimator::task_main()
 					_gps_start_time = hrt_absolute_time();
 
 					/* check if we had a GPS outage for a long time */
-					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
+					float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
+
+					const float pos_reset_threshold = 5.0f; // seconds
+
+					/* timeout of 5 seconds */
+					if (gps_elapsed > pos_reset_threshold) {
 						_ekf->ResetPosition();
 						_ekf->ResetVelocity();
 						_ekf->ResetStoredStates();
 					}
+					_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
 
 					/* fuse GPS updates */
 
@@ -1044,6 +1067,8 @@ FixedwingEstimator::task_main()
 
 					newDataGps = true;
 
+					last_gps = _gps.timestamp_position;
+
 				}
 
 			}
@@ -1052,8 +1077,15 @@ FixedwingEstimator::task_main()
 			orb_check(_baro_sub, &baro_updated);
 
 			if (baro_updated) {
+
+				hrt_abstime baro_last = _baro.timestamp;
+
 				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
 
+				float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
+
+				_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1));
+
 				_ekf->baroHgt = _baro.altitude;
 
 				if (!_baro_init) {
@@ -1114,6 +1146,19 @@ FixedwingEstimator::task_main()
 				newDataMag = false;
 			}
 
+			orb_check(_distance_sub, &newRangeData);
+
+			if (newRangeData) {
+				orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
+
+				if (_distance.valid) {
+					_ekf->rngMea = _distance.distance;
+					_distance_last_valid = _distance.timestamp;
+				} else {
+					newRangeData = false;
+				}
+			}
+
 			/*
 			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
 			 */
@@ -1197,6 +1242,7 @@ FixedwingEstimator::task_main()
 				} else if (_ekf->statesInitialised) {
 
 					// We're apparently initialized in this case now
+					// check (and reset the filter as needed)
 					int check = check_filter_state();
 
 					if (check) {
@@ -1206,21 +1252,7 @@ FixedwingEstimator::task_main()
 
 					// Run the strapdown INS equations every IMU update
 					_ekf->UpdateStrapdownEquationsNED();
-	#if 0
-					// debug code - could be tunred into a filter mnitoring/watchdog function
-					float tempQuat[4];
 
-					for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
-
-					quat2eul(eulerEst, tempQuat);
-
-					for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
-
-					if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
-
-					if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
-
-	#endif
 					// store the predicted states for subsequent use by measurement fusion
 					_ekf->StoreStates(IMUmsec);
 					// Check if on ground - status is used by covariance prediction
@@ -1334,6 +1366,13 @@ FixedwingEstimator::task_main()
 						_ekf->fuseVtasData = false;
 					}
 
+					if (newRangeData) {
+						_ekf->fuseRngData = true;
+						_ekf->useRangeFinder = true;
+						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
+						_ekf->GroundEKF();
+					}
+
 
 					// Output results
 					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
@@ -1447,6 +1486,10 @@ FixedwingEstimator::task_main()
 							_global_pos.vel_d = _local_pos.vz;
 						}
 
+						/* terrain altitude */
+						_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
+						_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
+						(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
 
 						_global_pos.yaw = _local_pos.yaw;
 
@@ -1467,8 +1510,10 @@ 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.windspeed_north = _ekf->windSpdFiltNorth;
+							_wind.windspeed_east = _ekf->windSpdFiltEast;
+							// XXX we need to do something smart about the covariance here
+							// but we default to the estimate covariance for now
 							_wind.covariance_north = _ekf->P[14][14];
 							_wind.covariance_east = _ekf->P[15][15];
 

src/modules/ekf_att_pos_estimator/estimator_23states.h

@@ -80,6 +80,14 @@ public:
         airspeedMeasurementSigma = 1.4f;
         gyroProcessNoise = 1.4544411e-2f;
         accelProcessNoise = 0.5f;
+
+        gndHgtSigma  = 0.1f; // terrain gradient 1-sigma
+        R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
+        flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
+        auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
+        rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
+        minFlowRng = 0.01f; //minimum range between ground and flow sensor
+        moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
     }
 
     struct mag_state_struct {
@@ -116,13 +124,16 @@ public:
     float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
     uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
 
+    // Times
+    uint64_t lastVelPosFusion;  // the time of the last velocity fusion, in the standard time unit of the filter
+
     float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
     float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
     float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
     float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
     float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
     float statesAtRngTime[n_states]; // filter states at the effective measurement time
-    float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
+    float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
 
     Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
     Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@@ -140,7 +151,16 @@ public:
     Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
     Vector3f dVelIMU;
     Vector3f dAngIMU;
-    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
+    float dtIMUfilt; // average time between IMU measurements (sec)
+    float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
+    float dtVelPosFilt; // average time between position / velocity fusion steps
+    float dtHgtFilt; // average time between height measurement updates
+    float dtGpsFilt; // average time between gps measurement updates
+    float windSpdFiltNorth; // average wind speed north component
+    float windSpdFiltEast; // average wind speed east component
+    float windSpdFiltAltitude; // the last altitude used to filter wind speed
+    float windSpdFiltClimb; // filtered climb rate
     uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
     float innovVelPos[6]; // innovation output
     float varInnovVelPos[6]; // innovation variance output
@@ -192,7 +212,8 @@ public:
 
     bool inhibitWindStates; // true when wind states and covariances are to remain constant
     bool inhibitMagStates;  // true when magnetic field states and covariances are to remain constant
-    bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
+    bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
+    bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
 
     bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
     bool staticMode;    ///< boolean true if no position feedback is fused
@@ -211,6 +232,30 @@ public:
 
     unsigned storeIndex;
 
+    // Optical Flow error estimation
+    float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
+
+    // Two state EKF used to estimate focal length scale factor and terrain position
+    float Popt[2][2];                       // state covariance matrix
+    float flowStates[2];                    // flow states [scale factor, terrain position]
+    float prevPosN;                         // north position at last measurement
+    float prevPosE;                         // east position at last measurement
+    float auxFlowObsInnov[2];               // optical flow observation innovations from focal length scale factor estimator
+    float auxFlowObsInnovVar[2];            // innovation variance for optical flow observations from focal length scale factor estimator
+    float fScaleFactorVar;                  // optical flow sensor focal length scale factor variance
+    Mat3f Tnb_flow;                         // Transformation matrix from nav to body at the time fo the optical flow measurement
+    float R_LOS;                            // Optical flow observation noise variance (rad/sec)^2
+    float auxFlowTestRatio[2];              // ratio of X and Y flow observation innovations to fault threshold
+    float auxRngTestRatio;                  // ratio of range observation innovations to fault threshold
+    float flowInnovGate;                    // number of standard deviations used for the innovation consistency check
+    float auxFlowInnovGate;                 // number of standard deviations applied to the optical flow innovation consistency check
+    float rngInnovGate;                     // number of standard deviations used for the innovation consistency check
+    float minFlowRng;                       // minimum range over which to fuse optical flow measurements
+    float moCompR_LOS;                      // scaler from sensor gyro rate to uncertainty in LOS rate
+
+void updateDtGpsFilt(float dt);
+
+void updateDtHgtFilt(float dt);
 
 void  UpdateStrapdownEquationsNED();
 
@@ -226,6 +271,8 @@ void FuseRangeFinder();
 
 void FuseOptFlow();
 
+void GroundEKF();
+
 void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
 
 void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
@@ -268,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
 static float sq(float valIn);
 
+static float maxf(float valIn1, float valIn2);
+
+static float min(float valIn1, float valIn2);
+
 void OnGroundCheck();
 
 void CovarianceInit();
@@ -300,6 +351,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
 
 protected:
 
+void updateDtVelPosFilt(float dt);
+
 bool FilterHealthy();
 
 bool GyroOffsetsDiverged();
@@ -314,3 +367,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
 
 uint32_t millis();
 
+uint64_t getMicros();
+

src/modules/sdlog2/sdlog2.c

@@ -1432,6 +1432,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
 			log_msg.body.log_GPOS.eph = buf.global_pos.eph;
 			log_msg.body.log_GPOS.epv = buf.global_pos.epv;
+			if (buf.global_pos.terrain_alt_valid) {
+				log_msg.body.log_GPOS.terrain_alt = buf.global_pos.terrain_alt;
+			} else {
+				log_msg.body.log_GPOS.terrain_alt = -1.0f;
+			}
 			LOGBUFFER_WRITE_AND_COUNT(GPOS);
 		}
 
@@ -1464,7 +1469,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw;
 			LOGBUFFER_WRITE_AND_COUNT(VICN);
 		}
-		
+
 		/* --- VISION POSITION --- */
 		if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
 			log_msg.msg_type = LOG_VISN_MSG;

src/modules/sdlog2/sdlog2_messages.h

@@ -220,6 +220,7 @@ struct log_GPOS_s {
 	float vel_d;
 	float eph;
 	float epv;
+	float terrain_alt;
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
@@ -449,7 +450,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
-	LOG_FORMAT(GPOS, "LLffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
+	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),

src/modules/uORB/topics/vehicle_global_position.h

@@ -72,6 +72,8 @@ struct vehicle_global_position_s {
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
 	float eph;			/**< Standard deviation of position estimate horizontally */
 	float epv;			/**< Standard deviation of position vertically */
+	float terrain_alt;		/**< Terrain altitude in m, WGS84 */
+	bool terrain_alt_valid;		/**< Terrain altitude estimate is valid */
 };
 
 /**