Add airspeed sensor to sensor_simulator

.gitignore

@@ -1,6 +1,8 @@
 *.DS_Store
 *.gcov
 *~
+*.orig
+
 .cache/
 build/
 

test/sensor_simulator/.gitignore

@@ -0,0 +1 @@
+__pycache__/

test/sensor_simulator/CMakeLists.txt

@@ -43,6 +43,7 @@ set(SRCS
 	flow.cpp
 	range_finder.cpp
 	vio.cpp
+	airspeed.cpp
    )
 
 add_library(ecl_sensor_sim ${SRCS})

test/sensor_simulator/airspeed.cpp

@@ -0,0 +1,32 @@
+#include "airspeed.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Airspeed::Airspeed(std::shared_ptr<Ekf> ekf):Sensor(ekf)
+{
+}
+
+Airspeed::~Airspeed()
+{
+}
+
+void Airspeed::send(uint64_t time)
+{
+	if(_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON)
+	{
+		float eas2tas = _true_airspeed_data / _indicated_airspeed_data;
+		_ekf->setAirspeedData(time, _true_airspeed_data, eas2tas);
+	}
+}
+
+void Airspeed::setData(float true_airspeed, float indicated_airspeed)
+{
+	_true_airspeed_data = true_airspeed;
+	_indicated_airspeed_data = indicated_airspeed;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

test/sensor_simulator/airspeed.h

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with Mag data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Airspeed: public Sensor
+{
+public:
+	Airspeed(std::shared_ptr<Ekf> ekf);
+	~Airspeed();
+
+	void setData(float true_airspeed, float eas2tas);
+
+private:
+	float _true_airspeed_data;
+	float _indicated_airspeed_data;
+
+	void send(uint64_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator

test/sensor_simulator/baro.cpp

@@ -13,7 +13,7 @@ Baro::~Baro()
 {
 }
 
-void Baro::send(uint32_t time)
+void Baro::send(uint64_t time)
 {
 	_ekf->setBaroData(time,_baro_data);
 }

test/sensor_simulator/baro.h

@@ -55,7 +55,7 @@ public:
 private:
 	float _baro_data;
 
-	void send(uint32_t time) override;
+	void send(uint64_t time) override;
 
 };
 

test/sensor_simulator/flow.cpp

@@ -13,8 +13,9 @@ Flow::~Flow()
 {
 }
 
-void Flow::send(uint32_t time)
+void Flow::send(uint64_t time)
 {
+	_flow_data.dt = time - _time_last_data_sent;
 	_ekf->setOpticalFlowData(time, &_flow_data);
 }
 

test/sensor_simulator/flow.h

@@ -56,7 +56,7 @@ public:
 private:
 	flow_message _flow_data;
 
-	void send(uint32_t time) override;
+	void send(uint64_t time) override;
 
 };
 

test/sensor_simulator/gps.cpp

@@ -13,7 +13,7 @@ Gps::~Gps()
 {
 }
 
-void Gps::send(uint32_t time)
+void Gps::send(uint64_t time)
 {
 	_gps_data.time_usec = time;
 	_ekf->setGpsData(time, _gps_data);
@@ -24,6 +24,28 @@ void Gps::setData(const gps_message& gps)
 	_gps_data = gps;
 }
 
+void Gps::setAltitude(int32_t alt)
+{
+	_gps_data.alt = alt;
+}
+
+void Gps::setLatitude(int32_t lat)
+{
+	_gps_data.lat = lat;
+}
+
+void Gps::setLongitude(int32_t lon)
+{
+	_gps_data.lon = lon;
+}
+
+void Gps::setVelocity(const Vector3f& vel)
+{
+	_gps_data.vel_ned[0] = vel(0);
+	_gps_data.vel_ned[1] = vel(1);
+	_gps_data.vel_ned[2] = vel(2);
+}
+
 void Gps::stepHeightByMeters(float hgt_change)
 {
 	_gps_data.alt += hgt_change * 1e3f;

test/sensor_simulator/gps.h

@@ -53,13 +53,17 @@ public:
 	void setData(const gps_message& gps);
 	void stepHeightByMeters(float hgt_change);
 	void stepHorizontalPositionByMeters(Vector2f hpos_change);
+	void setAltitude(int32_t alt);
+	void setLatitude(int32_t lat);
+	void setLongitude(int32_t lon);
+	void setVelocity(const Vector3f& vel);
 
 	gps_message getDefaultGpsData();
 
 private:
 	gps_message _gps_data;
 
-	void send(uint32_t time) override;
+	void send(uint64_t time) override;
 
 };
 

test/sensor_simulator/imu.cpp

@@ -13,7 +13,7 @@ Imu::~Imu()
 {
 }
 
-void Imu::send(uint32_t time)
+void Imu::send(uint64_t time)
 {
 	imuSample imu_sample;
 	imu_sample.time_us = time;

test/sensor_simulator/imu.h

@@ -58,7 +58,7 @@ private:
 	Vector3f _accel_data;
 	Vector3f _gyro_data;
 
-	void send(uint32_t time) override;
+	void send(uint64_t time) override;
 
 };
 

test/sensor_simulator/mag.cpp

@@ -13,7 +13,7 @@ Mag::~Mag()
 {
 }
 
-void Mag::send(uint32_t time)
+void Mag::send(uint64_t time)
 {
 	float mag[3];
 	_mag_data.copyTo(mag);

test/sensor_simulator/mag.h

@@ -55,7 +55,7 @@ public:
 private:
 	Vector3f _mag_data;
 
-	void send(uint32_t time) override;
+	void send(uint64_t time) override;
 
 };
 

test/sensor_simulator/range_finder.cpp

@@ -13,7 +13,7 @@ RangeFinder::~RangeFinder()
 {
 }
 
-void RangeFinder::send(uint32_t time)
+void RangeFinder::send(uint64_t time)
 {
 	_ekf->setRangeData(time, _range_data, _range_quality);
 }

test/sensor_simulator/range_finder.h

@@ -57,7 +57,7 @@ private:
 	float _range_data;
 	int8_t _range_quality;
 
-	void send(uint32_t time) override;
+	void send(uint64_t time) override;
 
 };
 

test/sensor_simulator/sensor.cpp

@@ -11,7 +11,7 @@ Sensor::~Sensor()
 {
 }
 
-void Sensor::update(uint32_t time)
+void Sensor::update(uint64_t time)
 {
 	if(should_send(time))
 	{
@@ -20,12 +20,12 @@ void Sensor::update(uint32_t time)
 	}
 }
 
-bool Sensor::should_send(uint32_t time) const
+bool Sensor::should_send(uint64_t time) const
 {
 	return _is_running && is_time_to_send(time);
 }
 
-bool Sensor::is_time_to_send(uint32_t time) const
+bool Sensor::is_time_to_send(uint64_t time) const
 {
 	return (time >= _time_last_data_sent) && ((time - _time_last_data_sent) >= _update_period);
 }

test/sensor_simulator/sensor.h

@@ -52,7 +52,7 @@ public:
 	Sensor(std::shared_ptr<Ekf> ekf);
 	virtual ~Sensor();
 
-	void update(uint32_t time);
+	void update(uint64_t time);
 
 	void setRateHz(uint32_t rate){ _update_period = uint32_t(1000000)/rate; }
 
@@ -67,17 +67,17 @@ protected:
 	std::shared_ptr<Ekf> _ekf;
 	// time in microseconds
 	uint32_t _update_period;
-	uint32_t _time_last_data_sent{0};
+	uint64_t _time_last_data_sent{0};
 
 	bool _is_running{false};
 
-	bool should_send(uint32_t time) const;
+	bool should_send(uint64_t time) const;
 
 	// Checks that the right amount time passed since last send data to fulfill rate
-	bool is_time_to_send(uint32_t time) const;
+	bool is_time_to_send(uint64_t time) const;
 
 	// call set*Data function of Ekf
-	virtual void send(uint32_t time) = 0;
+	virtual void send(uint64_t time) = 0;
 
 };
 

test/sensor_simulator/sensor_simulator.cpp

@@ -9,7 +9,8 @@ _baro(ekf),
 _gps(ekf),
 _flow(ekf),
 _rng(ekf),
-_vio(ekf)
+_vio(ekf),
+_airspeed(ekf)
 {
 	setSensorDataToDefault();
 	setSensorRateToDefault();
@@ -30,6 +31,7 @@ void SensorSimulator::setSensorDataToDefault()
 	_flow.setRateHz(50);
 	_rng.setRateHz(30);
 	_vio.setRateHz(30);
+	_airspeed.setRateHz(30); // TODO: check this rate
 }
 void SensorSimulator::setSensorRateToDefault()
 {
@@ -41,6 +43,7 @@ void SensorSimulator::setSensorRateToDefault()
 	_flow.setData(_flow.dataAtRest());
 	_rng.setData(0.2f, 100);
 	_vio.setData(_vio.dataAtRest());
+	_airspeed.setData(0.0f, 0.0f);
 }
 void SensorSimulator::startBasicSensor()
 {
@@ -57,10 +60,18 @@ void SensorSimulator::runSeconds(float duration_seconds)
 void SensorSimulator::runMicroseconds(uint32_t duration)
 {
 	// simulate in 1000us steps
-	const uint32_t start_time = _time;
+	const uint64_t start_time = _time;
 
-	for(;_time < start_time + duration; _time+=1000)
+	for(;_time < start_time + (uint64_t)duration; _time+=1000)
 	{
+		updateSensors();
+
+		_ekf->update();
+	}
+}
+
+void SensorSimulator::updateSensors()
+{
 	_imu.update(_time);
 	_mag.update(_time);
 	_baro.update(_time);
@@ -68,9 +79,7 @@ void SensorSimulator::runMicroseconds(uint32_t duration)
 	_flow.update(_time);
 	_rng.update(_time);
 	_vio.update(_time);
-
-		_ekf->update();
-	}
+	_airspeed.update(_time);
 }
 
 void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)

test/sensor_simulator/sensor_simulator.h

@@ -51,6 +51,7 @@
 #include "flow.h"
 #include "range_finder.h"
 #include "vio.h"
+#include "airspeed.h"
 #include "EKF/ekf.h"
 
 using namespace sensor_simulator::sensor;
@@ -61,16 +62,19 @@ class SensorSimulator
 private:
 	std::shared_ptr<Ekf> _ekf;
 
-	uint32_t _time {0};	// in microseconds
+	uint64_t _time {0};	// in microseconds
 
 	void setSensorDataToDefault();
 	void setSensorRateToDefault();
 	void startBasicSensor();
+	void updateSensors();
 
 public:
 	SensorSimulator(std::shared_ptr<Ekf> ekf);
 	~SensorSimulator();
 
+	uint64_t getTime() const{ return _time; };
+
 	void runSeconds(float duration_seconds);
 	void runMicroseconds(uint32_t duration);
 
@@ -86,6 +90,9 @@ public:
 	void startExternalVision(){ _vio.start(); }
 	void stopExternalVision(){ _vio.stop(); }
 
+	void startAirspeedSensor(){ _airspeed.start(); }
+	void stopAirspeedSensor(){ _airspeed.stop(); }
+
 	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
 	void simulateOrientation(Quatf orientation);
 
@@ -96,4 +103,5 @@ public:
 	Flow _flow;
 	RangeFinder _rng;
 	Vio _vio;
+	Airspeed _airspeed;
 };

test/sensor_simulator/vio.cpp

@@ -13,7 +13,7 @@ Vio::~Vio()
 {
 }
 
-void Vio::send(uint32_t time)
+void Vio::send(uint64_t time)
 {
 	_ekf->setExtVisionData(time, &_vio_data);
 }

test/sensor_simulator/vio.h

@@ -63,7 +63,7 @@ public:
 private:
 	ext_vision_message _vio_data;
 
-	void send(uint32_t time) override;
+	void send(uint64_t time) override;
 
 };