Sensor Replay (#717)

* Add primitive logging for Ekf * Add python script to extract sensor data from ULog * Add primitive sensor replay * Add iris_gps data for sensor replay * Add CI for functional change indication * Update sensor replay flow data type * update iris_gps_change indication * test: Update EKF replay test documentation Co-authored-by: Paul Riseborough <priseborough@users.noreply.github.com>
2026-06-27 11:20:35 +08:00 · 2020-02-01 10:41:12 +01:00
parent da752c9e30
commit e52e2b88ed
13 changed files with 29140 additions and 2 deletions
@@ -22,6 +22,88 @@ SensorSimulator::~SensorSimulator()

 }

+void SensorSimulator::loadSensorDataFromFile(std::string file_name)
+{
+	std::ifstream file(file_name);
+	std::string line;
+
+	while (!file.eof()) {
+		std::string timestamp;
+		std::string sensor_type;
+		std::string sensor_data;
+		sensor_info sensor_sample;
+
+		getline(file, timestamp, ',');
+
+		if (!timestamp.compare("")){ // empty line at end of file
+			break;
+		}
+		sensor_sample.timestamp = std::stoul(timestamp);
+
+		if(_replay_data.size() > 0) {
+			sensor_info last_sample = _replay_data.back();
+			if (sensor_sample.timestamp < last_sample.timestamp)
+			{
+				std::cout << "Timestamps not sorted ascendingly" << std::endl;
+				exit(-1);
+			}
+		}
+
+		getline(file, sensor_type, ',');
+		if (!sensor_type.compare("imu")) {
+			sensor_sample.sensor_type = sensor_info::IMU;
+		} else if (!sensor_type.compare("mag")) {
+			sensor_sample.sensor_type = sensor_info::MAG;
+
+		} else if (!sensor_type.compare("baro")) {
+			sensor_sample.sensor_type = sensor_info::BARO;
+
+		} else if (!sensor_type.compare("gps")) {
+			sensor_sample.sensor_type = sensor_info::GPS;
+
+		} else if (!sensor_type.compare("airspeed")) {
+			sensor_sample.sensor_type = sensor_info::AIRSPEED;
+
+		} else if (!sensor_type.compare("range")) {
+			sensor_sample.sensor_type = sensor_info::RANGE;
+
+		} else if (!sensor_type.compare("flow")) {
+			sensor_sample.sensor_type = sensor_info::FLOW;
+
+		} else if (!sensor_type.compare("vio")) {
+			sensor_sample.sensor_type = sensor_info::VISION;
+
+		} else if (!sensor_type.compare("landed")) {
+			sensor_sample.sensor_type = sensor_info::LANDING_STATUS;
+
+		} else {
+			std::cout << "Sensor type in file unknown" << std::endl;
+			exit(-1);
+		}
+
+		getline(file, sensor_data);
+		std::stringstream ss(sensor_data);
+		int8_t i = 0;
+		while( ss.good() )
+		{
+			if(i>=10){
+				std::cout << "sensor data bigger than expected" << std::endl;
+				exit(-1);
+			}
+			std::string value_string;
+			getline( ss, value_string, ',' );
+			if(!value_string.compare("")){
+				continue;
+			}
+			sensor_sample.sensor_data[i] = std::stod(value_string);
+			i++;
+		}
+		_replay_data.emplace_back(sensor_sample);
+	}
+	file.close();
+	_has_replay_data = true;
+}
+
 void SensorSimulator::setSensorDataToDefault()
 {
 	_imu.setRateHz(250);
@@ -82,6 +164,118 @@ void SensorSimulator::updateSensors()
 	_airspeed.update(_time);
 }

+void SensorSimulator::runReplaySeconds(float duration_seconds)
+{
+	runReplayMicroseconds( uint32_t(duration_seconds * 1e6f) );
+}
+
+void SensorSimulator::runReplayMicroseconds(uint32_t duration)
+{
+	if(!_has_replay_data) {
+		std::cout << "Can not run replay without replay data" << std::endl;
+		exit(-1);
+	}
+	// simulate in 1000us steps
+	const uint64_t start_time = _time;
+
+	for(;_time < start_time + duration; _time+=1000)
+	{
+		setSensorDataFromReplayData();
+
+		bool update_imu = _imu.should_send(_time);
+		updateSensors();
+
+		if(update_imu)
+		{
+			_ekf->update();
+		}
+	}
+}
+
+void SensorSimulator::setSensorDataFromReplayData()
+{
+	if(_replay_data.size() > 0) {
+		sensor_info sample = _replay_data[_current_replay_data_index];
+		while(sample.timestamp < _time)
+		{
+			setSingleReplaySample(sample);
+			if(_current_replay_data_index < _replay_data.size())
+			{
+				_current_replay_data_index ++;
+			} else {
+				break;
+			}
+			sample = _replay_data[_current_replay_data_index];
+		}
+	} else {
+		std::cerr << "Loaded replay data empty. Likely could not load replay data" << std::endl;
+		exit(-1);
+	}
+}
+
+void SensorSimulator::setSingleReplaySample(const sensor_info& sample)
+{
+	if (sample.sensor_type == sensor_info::IMU) {
+		Vector3f accel{(float) sample.sensor_data[0],
+				(float) sample.sensor_data[1],
+				(float) sample.sensor_data[2]};
+		Vector3f gyro{(float) sample.sensor_data[3],
+				(float) sample.sensor_data[4],
+				(float) sample.sensor_data[5]};
+		_imu.setData(accel, gyro);
+
+	} else if (sample.sensor_type == sensor_info::MAG) {
+		Vector3f mag{(float) sample.sensor_data[0],
+				(float) sample.sensor_data[1],
+				(float) sample.sensor_data[2]};
+		_mag.setData(mag);
+
+	} else if (sample.sensor_type == sensor_info::BARO) {
+		_baro.setData((float) sample.sensor_data[0]);
+
+	} else if (sample.sensor_type == sensor_info::GPS) {
+		_gps.setAltitude((int32_t) sample.sensor_data[0]);
+		_gps.setLatitude((int32_t) sample.sensor_data[1]);
+		_gps.setLongitude((int32_t) sample.sensor_data[2]);
+		_gps.setVelocity(Vector3f((float) sample.sensor_data[3],
+					  (float) sample.sensor_data[4],
+					  (float) sample.sensor_data[5]));
+
+	} else if (sample.sensor_type == sensor_info::AIRSPEED) {
+			_airspeed.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
+
+	} else if (sample.sensor_type == sensor_info::RANGE) {
+			_rng.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
+
+	} else if (sample.sensor_type == sensor_info::FLOW) {
+		flowSample flow_sample;
+		flow_sample.flow_xy_rad = Vector2f(sample.sensor_data[0],
+						 sample.sensor_data[1]);
+		flow_sample.gyro_xyz = Vector3f(sample.sensor_data[2],
+					       sample.sensor_data[3],
+					       sample.sensor_data[4]);
+		flow_sample.quality = sample.sensor_data[5];
+		_flow.setData(flow_sample);
+
+	} else if (sample.sensor_type == sensor_info::VISION) {
+		// sensor not yet implemented
+
+		// extVisionSample vision_sample;
+		// vision_sample.pos;
+		// vision_sample.quat;
+		// vision_sample.vel;
+		// _vio.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
+
+	} else if (sample.sensor_type == sensor_info::LANDING_STATUS) {
+		bool landed = sample.sensor_data[0];
+		_ekf->set_in_air_status(!landed);
+
+	} else {
+		printf("Unknown sensor type, can not set replay sample");
+		exit(-1);
+	}
+}
+
 void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
 {
 	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias,