From df6976c8d640b395220d46f5b1fd7ecfc8ae3e04 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 19 Apr 2013 16:20:40 +0200 Subject: Split diff pressure and airspeed. --- apps/commander/commander.c | 10 ++--- apps/drivers/ets_airspeed/ets_airspeed.cpp | 71 +++++------------------------- apps/sdlog/sdlog.c | 17 +++++-- apps/sensors/sensors.cpp | 59 +++++++++++++++++-------- apps/uORB/objects_common.cpp | 3 ++ apps/uORB/topics/differential_pressure.h | 11 ++--- apps/uORB/topics/sensor_combined.h | 2 + 7 files changed, 80 insertions(+), 93 deletions(-) (limited to 'apps') 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 -#include -#include #include +#include +#include +#include #include #include #include #include -#include /* for baro readings */ #include /* 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 #include #include +#include #include @@ -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 #include #include +#include #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]; @@ -330,6 +335,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. */ @@ -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")) @@ -887,6 +901,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() { @@ -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 */ }; /** -- cgit v1.2.3