Debugging, cleanup and added airspeed to HoTT telemetry.

5 files changed, 62 insertions, 20 deletions

diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp
index 943848d43..276f4bf59 100644
--- a/apps/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/apps/drivers/ets_airspeed/ets_airspeed.cpp
@@ -81,7 +81,11 @@
 #define READ_CMD	0x07		/* Read the data */
 	 
 /* Max measurement rate is 100Hz */
-#define CONVERSION_INTERVAL	(1000000 / 10)	/* microseconds */
+#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
+
+/* The Eagle Tree Airspeed V3 can only provide accurate readings
+   for speeds from 15km/h upwards. */
+#define MIN_ACCURATE_DIFF_PRES_PA 12
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -222,6 +226,8 @@ ETS_AIRSPEED::init()
 	/* allocate basic report buffers */
 	_num_reports = 2;
 	_reports = new struct differential_pressure_s[_num_reports];
+	for (int i = 0; i < _num_reports; i++)
+		_reports[i].max_differential_pressure_pa = 0;
 
 	if (_reports == nullptr)
 		goto out;
@@ -235,8 +241,6 @@ ETS_AIRSPEED::init()
 	if (_airspeed_pub < 0)
 		debug("failed to create airspeed sensor object. Did you start uOrb?");
 
-	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
-
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
@@ -454,13 +458,24 @@ ETS_AIRSPEED::collect()
 	
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
 
-	/* adjust if necessary */
-	diff_pres_pa -= _diff_pres_offset;
-	//log("measurement: %0.2f m/s", calc_indicated_airspeed((float)_reports[_next_report].diff_pressure));
+	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
 	
+	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { 
+		diff_pres_pa = 0;
+	} else {
+		diff_pres_pa -= _diff_pres_offset;	
+	}
+
+    // XXX we may want to smooth out the readings to remove noise.
+
 	_reports[_next_report].timestamp = hrt_absolute_time();
 	_reports[_next_report].differential_pressure_pa = diff_pres_pa;
 
+	// Track maximum differential pressure measured (so we can work out top speed).
+	if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) {
+		_reports[_next_report].max_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]);
 
@@ -684,7 +699,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -709,7 +724,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %0.3f pa", (double) report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", report.differential_pressure_pa);
 	}
 
 	errx(0, "PASS");
diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c
index 8bfb99773..0e466e820 100644
--- a/apps/hott_telemetry/messages.c
+++ b/apps/hott_telemetry/messages.c
@@ -42,9 +42,11 @@
 #include <string.h>
 #include <systemlib/systemlib.h>
 #include <unistd.h>
+#include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_combined.h>
 
+static int airspeed_sub = -1;
 static int battery_sub = -1;
 static int sensor_sub = -1;
 
@@ -52,6 +54,7 @@ void messages_init(void)
 {
 	battery_sub = orb_subscribe(ORB_ID(battery_status));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 }
 
 void build_eam_response(uint8_t *buffer, int *size)
@@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size)
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
+	/* get a local copy of the current sensor values */
+	struct airspeed_s airspeed;
+	memset(&airspeed, 0, sizeof(airspeed));
+	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
+
+	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6);
+	msg.speed_L = (uint8_t)speed & 0xff;
+	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+
 	msg.stop = STOP_BYTE;
 
 	memcpy(buffer, &msg, *size);
diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index da2dfcca6..0bab992a7 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -64,7 +64,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 2.5f);
+PARAM_DEFINE_INT32(SENS_DPRES_OFF, 1667);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index ab8818b40..fcd1d869f 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -192,7 +192,7 @@ private:
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
-		float airspeed_offset;
+		int diff_pres_offset_pa;
 
 		int rc_type;
 
@@ -241,7 +241,7 @@ private:
 		param_t accel_scale[3];
 		param_t mag_offset[3];
 		param_t mag_scale[3];
-		param_t airspeed_offset;
+		param_t diff_pres_offset_pa;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
@@ -497,7 +497,7 @@ Sensors::Sensors() :
 	_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
 
 	/* Differential pressure offset */
-	_parameter_handles.airspeed_offset = param_find("SENS_DPRES_OFF");
+	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
@@ -707,7 +707,7 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
 
 	/* Airspeed offset */
-	param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
+	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -910,15 +910,26 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 	if (updated) {
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
+		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
+		raw.differential_pressure_counter++;
+
 		float airspeed_true = calc_true_airspeed(_diff_pres.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(_diff_pres.differential_pressure_pa);
-		
-		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
-		raw.differential_pressure_counter++;
+
+		_airspeed.indicated_airspeed_m_s = airspeed_indicated;
+		_airspeed.true_airspeed_m_s = airspeed_true;
+
+		/* announce the airspeed if needed, just publish else */
+		if (_airspeed_pub > 0) {
+			orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
+
+		} else {
+			_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
+		}
 	}
 }
 
@@ -1080,7 +1091,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					 */
 					if (voltage > 0.4f) {
 
-						float diff_pres_pa = voltage * 1000.0f - _parameters.airspeed_offset; //for MPXV7002DP sensor
+						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
 
 						_diff_pres.timestamp = hrt_absolute_time();
 						_diff_pres.differential_pressure_pa = diff_pres_pa;
@@ -1330,6 +1341,7 @@ Sensors::task_main()
 	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -1405,6 +1417,8 @@ Sensors::task_main()
 		/* check battery voltage */
 		adc_poll(raw);
 
+		diff_pres_poll(raw);
+
 		/* Inform other processes that new data is available to copy */
 		if (_publishing)
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h
index ac5220619..8ce85213b 100644
--- a/apps/uORB/topics/differential_pressure.h
+++ b/apps/uORB/topics/differential_pressure.h
@@ -52,9 +52,10 @@
  * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
-	float		differential_pressure_pa;	/**< Differential pressure reading */
-	float		voltage;					/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	uint64_t	timestamp;						/**< microseconds since system boot, needed to integrate */
+	uint16_t	differential_pressure_pa;		/**< Differential pressure reading */
+	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
+	float		voltage;						/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
 
 };