Refactor velocity reset (#788)

* Refactor velocity reset

* Add unit tests for velocity resets

* Expand updates to vertical buffer to velocity resets outside of resetHeight

* Improve matrix library usage

* Improve naming of vertical output samples

* Fix update of output_vert_new during reset

* Improve naming of vertical output samples 2

test/sensor_simulator/range_finder.cpp

@@ -17,7 +17,7 @@ void RangeFinder::send(uint64_t time)
 {
 	_range_sample.time_us = time;
 	_ekf->setRangeData(_range_sample);
-	_ekf->set_rangefinder_limits(0.2f, 20.f); // This usually comes from the driver
+	_ekf->set_rangefinder_limits(_min_distance, _max_distance);
 }
 
 void RangeFinder::setData(float range_data_meters, int8_t range_quality)
@@ -26,5 +26,11 @@ void RangeFinder::setData(float range_data_meters, int8_t range_quality)
 	_range_sample.quality = range_quality;
 }
 
+void RangeFinder::setLimits(float min_distance_m, float max_distance_m)
+{
+	_min_distance = min_distance_m;
+	_max_distance = max_distance_m;
+}
+
 } // namespace sensor
 } // namespace sensor_simulator

test/sensor_simulator/range_finder.h

@@ -51,12 +51,14 @@ public:
 	~RangeFinder();
 
 	void setData(float range_data, int8_t range_quality);
+	void setLimits(float min_distance_m, float max_distance_m);
 
 private:
 	rangeSample _range_sample {};
+	float _min_distance {0.2f};
+	float _max_distance {20.0f};
 
 	void send(uint64_t time) override;
-
 };
 
 } // namespace sensor

test/sensor_simulator/sensor_simulator.cpp

@@ -104,7 +104,7 @@ void SensorSimulator::loadSensorDataFromFile(std::string file_name)
 	_has_replay_data = true;
 }
 
-void SensorSimulator::setSensorDataToDefault()
+void SensorSimulator::setSensorRateToDefault()
 {
 	_imu.setRateHz(250);
 	_mag.setRateHz(80);
@@ -115,7 +115,7 @@ void SensorSimulator::setSensorDataToDefault()
 	_vio.setRateHz(30);
 	_airspeed.setRateHz(100);
 }
-void SensorSimulator::setSensorRateToDefault()
+void SensorSimulator::setSensorDataToDefault()
 {
 	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
 		     Vector3f{0.0f,0.0f,0.0f});