diff options
author | Simon Wilks <sjwilks@gmail.com> | 2013-04-21 01:29:07 +0200 |
---|---|---|
committer | Simon Wilks <sjwilks@gmail.com> | 2013-04-21 01:29:07 +0200 |
commit | 48f815860b5900f3770486d88aea9084c75441e0 (patch) | |
tree | 3ce182a63d35aab7e3185cb028dca60874c7dd96 /apps/sensors/sensors.cpp | |
parent | 853ba612b132f0a8f41fae1dbadc68ef3960f0d0 (diff) | |
download | px4-firmware-48f815860b5900f3770486d88aea9084c75441e0.tar.gz px4-firmware-48f815860b5900f3770486d88aea9084c75441e0.tar.bz2 px4-firmware-48f815860b5900f3770486d88aea9084c75441e0.zip |
Debugging, cleanup and added airspeed to HoTT telemetry.
Diffstat (limited to 'apps/sensors/sensors.cpp')
-rw-r--r-- | apps/sensors/sensors.cpp | 30 |
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); |