From 0fe272c9b3fcac06a9f30f9ab19fd94ad9d969c8 Mon Sep 17 00:00:00 2001
From: tumbili <bapstr@ethz.ch>
Date: Fri, 4 Sep 2015 17:01:33 +0200
Subject: [PATCH] support simulated airspeed

---
 src/modules/simulator/simulator.cpp         |  9 +++++++++
 src/modules/simulator/simulator.h           | 11 +++++++++++
 src/modules/simulator/simulator_mavlink.cpp |  8 +++++++-
 3 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp
index f0345ba036..f8c8fcec0f 100644
--- a/src/modules/simulator/simulator.cpp
+++ b/src/modules/simulator/simulator.cpp
@@ -86,6 +86,11 @@ bool Simulator::getGPSSample(uint8_t *buf, int len)
 	return _gps.copyData(buf, len);
 }
 
+bool Simulator::getAirspeedSample(uint8_t *buf, int len)
+{
+	return _airspeed.copyData(buf, len);
+}
+
 void Simulator::write_MPU_data(void *buf) {
 	_mpu.writeData(buf);
 }
@@ -106,6 +111,10 @@ void Simulator::write_gps_data(void *buf) {
 	_gps.writeData(buf);
 }
 
+void Simulator::write_airspeed_data(void *buf) {
+	_airspeed.writeData(buf);
+}
+
 int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;
diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h
index dbb3d5b783..af4b8a746c 100644
--- a/src/modules/simulator/simulator.h
+++ b/src/modules/simulator/simulator.h
@@ -94,6 +94,13 @@ struct RawBaroData {
 };
 #pragma pack(pop)
 
+#pragma pack(push, 1)
+struct RawAirspeedData {
+	float temperature;
+	float diff_pressure;
+};
+#pragma pack(pop)
+
 #pragma pack(push, 1)
 struct RawGPSData {
  int32_t lat;
@@ -191,12 +198,14 @@ public:
 	bool getMPUReport(uint8_t *buf, int len);
 	bool getBaroSample(uint8_t *buf, int len);
 	bool getGPSSample(uint8_t *buf, int len);
+	bool getAirspeedSample(uint8_t *buf, int len);
 
 	void write_MPU_data(void *buf);
 	void write_accel_data(void *buf);
 	void write_mag_data(void *buf);
 	void write_baro_data(void *buf);
 	void write_gps_data(void *buf);
+	void write_airspeed_data(void *buf);
 
 	bool isInitialized() { return _initialized; }
 
@@ -207,6 +216,7 @@ private:
 	_baro(1),
 	_mag(1),
 	_gps(1),
+	_airspeed(1),
 	_accel_pub(nullptr),
 	_baro_pub(nullptr),
 	_gyro_pub(nullptr),
@@ -238,6 +248,7 @@ private:
 	simulator::Report<simulator::RawBaroData>	_baro;
 	simulator::Report<simulator::RawMagData>	_mag;
 	simulator::Report<simulator::RawGPSData>	_gps;
+	simulator::Report<simulator::RawAirspeedData> _airspeed;
 
 	// uORB publisher handlers
 	orb_advert_t _accel_pub;
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index ec3a7c8a24..ddbc79db76 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -187,11 +187,17 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
 
 	RawBaroData baro;
 	// calculate air pressure from altitude (valid for low altitude)
-	baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt;
+	baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar
 	baro.altitude = imu->pressure_alt;
 	baro.temperature = imu->temperature;
 
 	write_baro_data((void *)&baro);
+
+	RawAirspeedData airspeed;
+	airspeed.temperature = imu->temperature;
+	airspeed.diff_pressure = imu->diff_pressure;
+
+	write_airspeed_data((void *)&airspeed);
 }
 
 void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {