Split diff pressure and airspeed.

7 files changed, 80 insertions, 93 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 7c0a25f86..fcfffcfef 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -801,7 +801,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 	struct differential_pressure_s differential_pressure;
 
 	int calibration_counter = 0;
-	float airspeed_offset = 0.0f;
+	float diff_pres_offset = 0.0f;
 
 	while (calibration_counter < calibration_count) {
 
@@ -812,7 +812,7 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
-			airspeed_offset += differential_pressure.voltage;
+			diff_pres_offset += differential_pressure.differential_pressure_pa;
 			calibration_counter++;
 
 		} else if (poll_ret == 0) {
@@ -822,11 +822,11 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 		}
 	}
 
-	airspeed_offset = airspeed_offset / calibration_count;
+	diff_pres_offset = diff_pres_offset / calibration_count;
 
-	if (isfinite(airspeed_offset)) {
+	if (isfinite(diff_pres_offset)) {
 
-		if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
+		if (param_set(param_find("SENS_VAIR_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
 		}
 
diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp
index 58d016a30..860baa760 100644
--- a/apps/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp
@@ -61,16 +61,16 @@
 
 #include <arch/board/board.h>
 
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
 #include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/sensor_combined.h>		/* for baro readings */
 #include <uORB/topics/subsystem_info.h>
 
 /* Configuration Constants */
@@ -83,9 +83,6 @@
 /* Max measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 10)	/* microseconds */
 
-#define DIFF_PRESSURE_SCALE   1.0
-#define DIFF_PRESSURE_OFFSET  1673
-
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -96,9 +93,6 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-static int _sensor_sub = -1;
-
-
 class ETS_AIRSPEED : public device::I2C
 {
 public:
@@ -127,6 +121,7 @@ private:
 	bool						_sensor_ok;
 	int							_measure_ticks;
 	bool						_collect_phase;
+	int 						_differential_pressure_offset;
 	
 	orb_advert_t				_airspeed_pub;
 
@@ -195,7 +190,8 @@ ETS_AIRSPEED::ETS_AIRSPEED(int bus, int address) :
 	_airspeed_pub(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ETS_AIRSPEED_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "ETS_AIRSPEED_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "ETS_AIRSPEED_buffer_overflows"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "ETS_AIRSPEED_buffer_overflows")),
+	_differential_pressure_offset(0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -239,12 +235,7 @@ ETS_AIRSPEED::init()
 	if (_airspeed_pub < 0)
 		debug("failed to create airspeed sensor object. Did you start uOrb?");
 
-	_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	
-	if (_sensor_sub < 0) {
-		debug("failed to subscribe to sensor_combined object.");
-		return ret;
-	}
+	param_get(param_find("SENS_VAIR_OFF"), &_differential_pressure_offset);
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
@@ -462,38 +453,13 @@ ETS_AIRSPEED::collect()
 	}
 	
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
-	//log("val: %0.3f", (float)(diff_pressure));
 
 	/* adjust if necessary */
-	diff_pres_pa = DIFF_PRESSURE_SCALE * (diff_pres_pa - DIFF_PRESSURE_OFFSET);
+	diff_pres_pa -= _differential_pressure_offset;
 	//log("measurement: %0.2f m/s", calc_indicated_airspeed((float)_reports[_next_report].diff_pressure));
-
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
-
-	bool updated;
-	orb_check(_sensor_sub, &updated);
-	if (updated) {		
-		orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
-		printf("baro temp %3.6f\n", raw.baro_pres_mbar);
-	}
-	//if (raw.baro_temp_celcius > 0) 
-	//	log("baro temp %3.3f\n", (uint8_t) raw.baro_temp_celcius);
-
-	float airspeed_true = calc_true_airspeed(diff_pres_pa + raw.baro_pres_mbar*1e2f, 
-											 raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa
-	// XXX HACK - true temperature is much less than indicated temperature in baro,
-	// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
-
-	float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
 	
 	_reports[_next_report].timestamp = hrt_absolute_time();
-	_reports[_next_report].static_pressure_mbar = raw.baro_pres_mbar;
-	_reports[_next_report].differential_pressure_mbar = diff_pres_pa * 1e-2f;
-	_reports[_next_report].temperature_celcius = raw.baro_temp_celcius;
-	_reports[_next_report].indicated_airspeed_m_s = airspeed_indicated;
-	_reports[_next_report].true_airspeed_m_s = airspeed_true;
-	_reports[_next_report].voltage = 0;
+	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
 
 	/* announce the airspeed if needed, just publish else */
 	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]);
@@ -512,7 +478,6 @@ ETS_AIRSPEED::collect()
 
 	ret = OK;
 
-out:
 	perf_end(_sample_perf);
 	
 	return ret;
@@ -719,19 +684,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %0.3f mbar", report.differential_pressure_mbar);
-	warnx("indicated airspeed: %0.1f m/s", report.indicated_airspeed_m_s);
-	warnx("true airspeed: %0.1f m/s", report.true_airspeed_m_s);
-
-	struct sensor_combined_s raw;
-	memset(&raw, 0, sizeof(raw));
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-
-	//if (raw.baro_temp_celcius > 0) 
-	//	log("baro temp %3.3f\n", (uint8_t) raw.baro_temp_celcius);
-	warnx("temp: %3.5f", raw.baro_temp_celcius);
-	warnx("time:        %lld", report.timestamp);
+	warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -756,9 +709,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %0.3f mbar", report.differential_pressure_mbar);
-		warnx("indicated airspeed: %0.1f m/s", report.indicated_airspeed_m_s);
-		warnx("true airspeed: %0.1f m/s", report.true_airspeed_m_s);		warnx("time:        %lld", report.timestamp);
+		warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa);
 	}
 
 	errx(0, "PASS");
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index df8745d9f..46b232c34 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -71,6 +71,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
 
 #include <systemlib/systemlib.h>
 
@@ -444,6 +445,7 @@ int sdlog_thread_main(int argc, char *argv[])
 		struct optical_flow_s flow;
 		struct battery_status_s batt;
 		struct differential_pressure_s diff_pressure;
+		struct airspeed_s airspeed;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -462,6 +464,7 @@ int sdlog_thread_main(int argc, char *argv[])
 		int flow_sub;
 		int batt_sub;
 		int diff_pressure_sub;
+		int airspeed_sub;
 	} subs;
 
 	/* --- MANAGEMENT - LOGGING COMMAND --- */
@@ -563,6 +566,13 @@ int sdlog_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- AIRSPEED --- */
+	/* subscribe to ORB for airspeed */
+	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	fds[fdsc_count].fd = subs.airspeed_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;	
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -655,6 +665,7 @@ int sdlog_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
 				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
 				orb_copy(ORB_ID(differential_pressure), subs.diff_pressure_sub, &buf.diff_pressure);
+				orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
 				orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
 
 				/* if skipping is on or logging is disabled, ignore */
@@ -691,9 +702,9 @@ int sdlog_thread_main(int argc, char *argv[])
 					.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
 					.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
 					.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
-					.diff_pressure = buf.diff_pressure.differential_pressure_mbar,
-					.ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s,
-					.true_airspeed = buf.diff_pressure.true_airspeed_m_s
+					.diff_pressure = buf.diff_pressure.differential_pressure_pa,
+					.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
+					.true_airspeed = buf.airspeed.true_airspeed_m_s
 				};
 
 				/* put into buffer for later IO */
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index d725c7727..2cf3b92ef 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -77,6 +77,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
 
 #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20   /* 40 ms downtime at 500 Hz update rate   */
 #define ACC_HEALTH_COUNTER_LIMIT_ERROR  20   /* 40 ms downtime at 500 Hz update rate   */
@@ -156,6 +157,8 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
+	int     _airspeed_sub;		/**< airspeed subscription */
+	int		_differential_pressure_sub; /**< raw differential pressure subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
@@ -165,6 +168,7 @@ private:
 	orb_advert_t	_rc_pub;			/**< raw r/c control topic */
 	orb_advert_t	_battery_pub;			/**< battery status */
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
+	orb_advert_t	_differential_pressure_pub;			/**< differential_pressure */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -172,6 +176,7 @@ private:
 	struct battery_status_s _battery_status;	/**< battery status */
 	struct baro_report _barometer;			/**< barometer data */
 	struct differential_pressure_s _differential_pressure;
+	struct airspeed_s _airspeed;
 
 	struct {
 		float min[_rc_max_chan_count];
@@ -331,6 +336,14 @@ private:
 	void		baro_poll(struct sensor_combined_s &raw);
 
 	/**
+	 * Poll the differential pressure sensor for updated data.
+	 *
+	 * @param raw			Combined sensor data structure into which
+	 *				data should be returned.
+	 */
+	void		differential_pressure_poll(struct sensor_combined_s &raw);
+
+	/**
 	 * Check for changes in vehicle status.
 	 */
 	void		vehicle_status_poll();
@@ -398,6 +411,7 @@ Sensors::Sensors() :
 	_rc_pub(-1),
 	_battery_pub(-1),
 	_airspeed_pub(-1),
+	_differential_pressure_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
@@ -888,6 +902,27 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 }
 
 void
+Sensors::differential_pressure_poll(struct sensor_combined_s &raw)
+{
+	bool updated;
+	orb_check(_differential_pressure_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(differential_pressure), _differential_pressure_sub, &_differential_pressure);
+
+		float airspeed_true = calc_true_airspeed(_differential_pressure.differential_pressure_pa + raw.baro_pres_mbar*1e2f, 
+											 	 raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - 5.0f); //factor 1e2 for conversion from mBar to Pa
+		// XXX HACK - true temperature is much less than indicated temperature in baro,
+		// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
+
+		float airspeed_indicated = calc_indicated_airspeed(_differential_pressure.differential_pressure_pa);
+		
+		raw.differential_pressure_pa = _differential_pressure.differential_pressure_pa;
+		raw.differential_pressure_counter++;
+	}
+}
+
+void
 Sensors::vehicle_status_poll()
 {
 	struct vehicle_status_s vstatus;
@@ -1045,31 +1080,18 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					 */
 					if (voltage > 0.4f) {
 
-						float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
-
-						float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
-							_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
-						// XXX HACK - true temperature is much less than indicated temperature in baro,
-						// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
-
-						float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
-
-						//printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true);
+						float diff_pres_pa = voltage * 1000.0f - _parameters.airspeed_offset; //for MPXV7002DP sensor
 
 						_differential_pressure.timestamp = hrt_absolute_time();
-						_differential_pressure.static_pressure_mbar = _barometer.pressure;
-						_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
-						_differential_pressure.temperature_celcius = _barometer.temperature;
-						_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
-						_differential_pressure.true_airspeed_m_s = airspeed_true;
+						_differential_pressure.differential_pressure_pa = diff_pres_pa;
 						_differential_pressure.voltage = voltage;
 
 						/* announce the airspeed if needed, just publish else */
-						if (_airspeed_pub > 0) {
-							orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_differential_pressure);
+						if (_differential_pressure_pub > 0) {
+							orb_publish(ORB_ID(differential_pressure), _differential_pressure_pub, &_differential_pressure);
 
 						} else {
-							_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure);
+							_differential_pressure_pub = orb_advertise(ORB_ID(differential_pressure), &_differential_pressure);
 						}
 					}
 				}
@@ -1334,6 +1356,7 @@ Sensors::task_main()
 	gyro_poll(raw);
 	mag_poll(raw);
 	baro_poll(raw);
+	differential_pressure_poll(raw);
 
 	parameter_update_poll(true /* forced */);
 
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index 136375140..4197f6fb2 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -122,6 +122,9 @@ ORB_DEFINE(optical_flow, struct optical_flow_s);
 #include "topics/omnidirectional_flow.h"
 ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s);
 
+#include "topics/airspeed.h"
+ORB_DEFINE(airspeed, struct airspeed_s);
+
 #include "topics/differential_pressure.h"
 ORB_DEFINE(differential_pressure, struct differential_pressure_s);
 
diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h
index d5e4bf37e..ac5220619 100644
--- a/apps/uORB/topics/differential_pressure.h
+++ b/apps/uORB/topics/differential_pressure.h
@@ -49,16 +49,13 @@
  */
 
 /**
- * Differential pressure and airspeed
+ * Differential pressure.
  */
 struct differential_pressure_s {
 	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
-	float   	static_pressure_mbar;		/**< Static / environment pressure */
-	float		differential_pressure_mbar;	/**< Differential pressure reading */
-	float		temperature_celcius;		/**< ambient temperature in celcius, -1 if unknown */
-	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
-	float		voltage;					/**< Voltage from the airspeed sensor (voltage divider already compensated) */
+	float		differential_pressure_pa;	/**< Differential pressure reading */
+	float		voltage;					/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+
 };
 
 /**
diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h
index 961ee8b4a..ad88e4b6e 100644
--- a/apps/uORB/topics/sensor_combined.h
+++ b/apps/uORB/topics/sensor_combined.h
@@ -103,6 +103,8 @@ struct sensor_combined_s {
 	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
 	uint32_t baro_counter;			/**< Number of raw baro measurements taken        */
 
+	float differential_pressure_pa;			/**< Airspeed sensor differential pressure                  */ 
+	uint32_t differential_pressure_counter;		/**< Number of raw differential pressure measurements taken */
 };
 
 /**