Merged master

2 files changed, 20 insertions, 11 deletions

diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c
index 9f23ebbba..a1ef9d136 100644
--- a/apps/sensors/sensor_params.c
+++ b/apps/sensors/sensor_params.c
@@ -64,6 +64,8 @@ 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_VAIR_OFF, 2.5f);
+
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 19cb13d06..9b95c0939 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -187,6 +187,7 @@ private:
 		float mag_scale[3];
 		float accel_offset[3];
 		float accel_scale[3];
+		float airspeed_offset;
 
 		int rc_type;
 
@@ -235,6 +236,7 @@ private:
 		param_t accel_scale[3];
 		param_t mag_offset[3];
 		param_t mag_scale[3];
+		param_t airspeed_offset;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
@@ -480,6 +482,9 @@ Sensors::Sensors() :
 	_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
 	_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
 
+	/*Airspeed offset */
+	_parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF");
+
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
 	/* fetch initial parameter values */
@@ -687,6 +692,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1]));
 	param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
 
+	/* Airspeed offset */
+	param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
+
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
 		warnx("Failed updating voltage scaling param");
@@ -993,7 +1001,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 		/* look for battery channel */
 
 		for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
-
+			
 			if (ret >= (int)sizeof(buf_adc[0])) {
 
 				if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1025,8 +1033,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 
 					/* calculate airspeed, raw is the difference from */
-
-					float voltage = buf_adc[i].am_data / 4096.0f;
+					float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
@@ -1034,24 +1041,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					 */
 					if (voltage > 0.4f) {
 
-						float pres_raw = fabsf(voltage - (3.3f / 2.0f));
-						float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
+						float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
 
-						float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure,
-							_barometer.pressure, _barometer.temperature - 5.0f);
+						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(pres_mbar + _barometer.pressure,
-							_barometer.pressure, _barometer.temperature - 5.0f);
-						// XXX HACK
+						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);
 
 						_differential_pressure.timestamp = hrt_absolute_time();
 						_differential_pressure.static_pressure_mbar = _barometer.pressure;
-						_differential_pressure.differential_pressure_mbar = pres_mbar;
+						_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.voltage = voltage;
 
 						/* announce the airspeed if needed, just publish else */
 						if (_airspeed_pub > 0) {