Expand test framework and test cases (#685)

* Fix comment

* Ekf wrapper for testing

Add utility function for accessing information in the ekf object

* Add step function for Gps sensor

* Add RangeFinder and Flow to simulated sensors

* Add first fusion logic tests

* Add units to function name

* Use EXPECT_TRUE

* Adding missing qualifiers

* Improve EXPECT_ calls

* Improve naming

test/sensor_simulator/ekf_wrapper.cpp

@@ -0,0 +1,71 @@
+#include "ekf_wrapper.h"
+
+EkfWrapper::EkfWrapper(std::shared_ptr<Ekf> ekf):
+_ekf{ekf}
+{
+	_ekf_params = _ekf->getParamHandle();
+}
+
+EkfWrapper::~EkfWrapper()
+{
+}
+
+void EkfWrapper::enableGpsFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_GPS;
+}
+
+void EkfWrapper::disableGpsFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_GPS;
+}
+
+bool EkfWrapper::isIntendingGpsFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.gps;
+}
+
+void EkfWrapper::enableFlowFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_OF;
+}
+
+void EkfWrapper::disableFlowFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_OF;
+}
+
+bool EkfWrapper::isIntendingFlowFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.opt_flow;
+}
+
+Vector3f EkfWrapper::getPosition() const
+{
+	float temp[3];
+	_ekf->get_position(temp);
+	return Vector3f(temp);
+}
+Vector3f EkfWrapper::getVelocity() const
+{
+	float temp[3];
+	_ekf->get_velocity(temp);
+	return Vector3f(temp);
+}
+Vector3f EkfWrapper::getAccelBias() const
+{
+	float temp[3];
+	_ekf->get_accel_bias(temp);
+	return Vector3f(temp);
+}
+
+Vector3f EkfWrapper::getGyroBias() const
+{
+	float temp[3];
+	_ekf->get_gyro_bias(temp);
+	return Vector3f(temp);
+}