diff --git a/apps/drivers/ets_airspeed/ets_airspeed.cpp b/apps/drivers/ets_airspeed/ets_airspeed.cpp index 943848d438..276f4bf593 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 8bfb997737..0e466e8209 100644 --- a/apps/hott_telemetry/messages.c +++ b/apps/hott_telemetry/messages.c @@ -42,9 +42,11 @@ #include #include #include +#include #include #include +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 da2dfcca67..0bab992a78 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 ab8818b40c..fcd1d869f9 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 ac52206192..8ce85213be 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) */ };