ekf2 app:

- support use of replay

src/modules/ekf2/ekf2_main.cpp

@@ -76,6 +76,7 @@
 #include <uORB/topics/ekf2_innovations.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/ekf2_replay.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -111,6 +112,8 @@ public:
 	 */
 	int		start();
 
+	void 	set_replay_mode(bool replay) {_replay_mode = true;};
+
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	void		task_main();
@@ -121,8 +124,10 @@ public:
 
 private:
 	static constexpr float _dt_max = 0.02;
-	bool		_task_should_exit = false;		/**< if true, task should exit */
+	bool		_task_should_exit = false;
-	int		_control_task = -1;			/**< task handle for task */
+	int		_control_task = -1;			// task handle for task
+	bool 	_replay_mode;	// should we use replay data from a log
+	int 	_publish_replay_mode;	// defines if we should publish replay messages
 
 	int		_sensors_sub = -1;
 	int		_gps_sub = -1;
@@ -139,6 +144,7 @@ private:
 	orb_advert_t _vehicle_global_position_pub;
 	orb_advert_t _estimator_status_pub;
 	orb_advert_t _estimator_innovations_pub;
+	orb_advert_t _replay_pub;
 
 	/* Low pass filter for attitude rates */
 	math::LowPassFilter2p _lp_roll_rate;
@@ -189,6 +195,7 @@ private:
 	control::BlockParamFloat *_requiredGDoP;        // maximum acceptable geometric dilution of precision
 	control::BlockParamFloat *_requiredHdrift;      // maximum acceptable horizontal drift speed (m/s)
 	control::BlockParamFloat *_requiredVdrift;      // maximum acceptable vertical drift speed (m/s)
+	control::BlockParamInt *_param_record_replay_msg; // indicates if we want to record ekf2 replay messages
 
 	int update_subscriptions();
 
@@ -196,12 +203,15 @@ private:
 
 Ekf2::Ekf2():
 	SuperBlock(NULL, "EKF"),
+	_replay_mode(false),
+	_publish_replay_mode(0),
 	_att_pub(nullptr),
 	_lpos_pub(nullptr),
 	_control_state_pub(nullptr),
 	_vehicle_global_position_pub(nullptr),
 	_estimator_status_pub(nullptr),
 	_estimator_innovations_pub(nullptr),
+	_replay_pub(nullptr),
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
 	_lp_yaw_rate(250.0f, 20.0f),
@@ -239,7 +249,8 @@ Ekf2::Ekf2():
 	_requiredNsats(new control::BlockParamInt(this, "EKF2_REQ_NSATS", false, &_params->req_nsats)),
 	_requiredGDoP(new control::BlockParamFloat(this, "EKF2_REQ_GDOP", false, &_params->req_gdop)),
 	_requiredHdrift(new control::BlockParamFloat(this, "EKF2_REQ_HDRIFT", false, &_params->req_hdrift)),
-	_requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift))
+	_requiredVdrift(new control::BlockParamFloat(this, "EKF2_REQ_VDRIFT", false, &_params->req_vdrift)),
+	_param_record_replay_msg(new control::BlockParamInt(this, "EKF2_REC_RPL", false, &_publish_replay_mode))
 {
 
 }
@@ -273,7 +284,13 @@ void Ekf2::task_main()
 	// initialise parameter cache
 	updateParams();
 
+	// initialize data structures outside of loop
+	// because they will else not always be
+	// properly populated
+	sensor_combined_s sensors = {};
 	vehicle_gps_position_s gps = {};
+	airspeed_s airspeed = {};
+	vehicle_control_mode_s vehicle_control_mode = {};
 
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -307,12 +324,7 @@ void Ekf2::task_main()
 		bool control_mode_updated = false;
 		bool vehicle_status_updated = false;
 
-		sensor_combined_s sensors = {};
-		airspeed_s airspeed = {};
-		vehicle_control_mode_s vehicle_control_mode = {};
-
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
-
 		// update all other topics if they have new data
 		orb_check(_gps_sub, &gps_updated);
 
@@ -328,6 +340,7 @@ void Ekf2::task_main()
 
 		// Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status
 		// TODO implement a global vehicle on-ground/in-air check
+		// XXX: Look at the land_detection module!
 		orb_check(_control_mode_sub, &control_mode_updated);
 
 		if (control_mode_updated) {
@@ -335,7 +348,14 @@ void Ekf2::task_main()
 			_ekf->set_arm_status(vehicle_control_mode.flag_armed);
 		}
 
-		hrt_abstime now = hrt_absolute_time();
+		// in replay mode we are getting the actual timestamp from the sensor topic
+		hrt_abstime now = 0;
+		if (_replay_mode) {
+			now = sensors.timestamp;
+		} else {
+			now = hrt_absolute_time();
+		}
+
 		// push imu data into estimator
 		_ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
 				 &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
@@ -425,9 +445,8 @@ void Ekf2::task_main()
 
 		// Position of local NED origin in GPS / WGS84 frame
 		struct map_projection_reference_s ekf_origin = {};
-		_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt);
+		_ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); 	// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
-		lpos.xy_global =
+		lpos.xy_global = _ekf->position_is_valid();
-			_ekf->position_is_valid();          // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
 		lpos.z_global = true;                                // true if z is valid and has valid global reference (ref_alt)
 		lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
 		lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
@@ -572,8 +591,40 @@ void Ekf2::task_main()
 			_mag_declination_deg->set(decl_deg);
 		}
 
-		_prev_motors_armed = vehicle_control_mode.flag_armed;
+		// publish replay message if in replay mode
+		bool publish_replay_message = (bool)_param_record_replay_msg->get();
+		if (publish_replay_message) {
+			struct ekf2_replay_s replay = {};
+			replay.time_ref = now;
+			replay.gyro_integral_dt = sensors.gyro_integral_dt[0];
+			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt[0];
+			replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0];
+			replay.baro_timestamp = sensors.baro_timestamp[0];
+			memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad));
+			memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], sizeof(replay.accelerometer_integral_m_s));
+			memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga));
+			replay.baro_alt_meter = sensors.baro_alt_meter[0];
+			replay.time_usec = gps.timestamp_position;
+			replay.time_usec_vel = gps.timestamp_velocity;
+			replay.lat = gps.lat;
+			replay.lon = gps.lon;
+			replay.alt = gps.alt;
+			replay.fix_type = gps.fix_type;
+			replay.eph = gps.eph;
+			replay.epv = gps.epv;
+			replay.vel_m_s = gps.vel_m_s;
+			replay.vel_n_m_s = gps.vel_n_m_s;
+			replay.vel_e_m_s = gps.vel_e_m_s;
+			replay.vel_d_m_s = gps.vel_d_m_s;
+			replay.vel_ned_valid = gps.vel_ned_valid;
 
+			if (_replay_pub == nullptr) {
+				_replay_pub = orb_advertise(ORB_ID(ekf2_replay), &replay);
+
+			} else {
+				orb_publish(ORB_ID(ekf2_replay), _replay_pub, &replay);
+			}
+		}
 	}
 
 	delete ekf2::instance;
@@ -621,6 +672,12 @@ int ekf2_main(int argc, char *argv[])
 
 		ekf2::instance = new Ekf2();
 
+		if (argc >= 3) {
+			if (!strcmp(argv[2], "--replay")) {
+				ekf2::instance->set_replay_mode(true);
+			}
+		}
+
 		if (ekf2::instance == nullptr) {
 			PX4_WARN("alloc failed");
 			return 1;

src/modules/ekf2/ekf2_params.c

@@ -378,3 +378,13 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f);
  * @unit SD
  */
 PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f);
+
+/**
+ * A value of 1 indicates that the ekf2 module will publish
+ * replay messages for logging.
+ *
+ * @group EKF2
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);