Debugging, cleanup and added airspeed to HoTT telemetry.

1 files changed, 22 insertions, 8 deletions

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);