diff options
-rw-r--r-- | src/drivers/ets_airspeed/ets_airspeed.cpp | 5 | ||||
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 9 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 4 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2.c | 1 | ||||
-rw-r--r-- | src/modules/sdlog2/sdlog2_messages.h | 3 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 13 | ||||
-rw-r--r-- | src/modules/uORB/topics/differential_pressure.h | 7 | ||||
-rw-r--r-- | src/modules/uORB/topics/sensor_combined.h | 2 |
8 files changed, 33 insertions, 11 deletions
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d27ab9727..d873a1132 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -176,11 +176,14 @@ ETSAirspeed::collect() _max_differential_pressure_pa = diff_pres_pa; } - // XXX we may want to smooth out the readings to remove noise. differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.differential_pressure_pa = (float)diff_pres_pa; + + // XXX we may want to smooth out the readings to remove noise. + report.differential_pressure_filtered_pa = (float)diff_pres_pa; + report.temperature = -1000.0f; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index beca28e7a..2c3efdc35 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -79,6 +79,8 @@ #include <systemlib/param/param.h> #include <systemlib/perf_counter.h> +#include <mathlib/math/filter/LowPassFilter2p.hpp> + #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> @@ -99,6 +101,8 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ +#define MEAS_RATE 100.0f +#define MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ class MEASAirspeed : public Airspeed @@ -116,6 +120,7 @@ protected: virtual int measure(); virtual int collect(); + math::LowPassFilter2p _filter; }; /* @@ -124,7 +129,8 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) { } @@ -212,6 +218,7 @@ MEASAirspeed::collect() report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 489d2bdcb..d7e300670 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -634,13 +634,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ee3ec7216..ad709d27d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1021,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa; LOGBUFFER_WRITE_AND_COUNT(SENS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index e27518aa0..fe500ad5f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -92,6 +92,7 @@ struct log_SENS_s { float baro_alt; float baro_temp; float diff_pres; + float diff_pres_filtered; }; /* --- LPOS - LOCAL POSITION --- */ @@ -306,7 +307,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), + LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 12ed55480..f890c4c7f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1031,11 +1031,14 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; raw.differential_pressure_timestamp = _diff_pres.timestamp; + raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - _airspeed.timestamp = hrt_absolute_time(); - _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + + _airspeed.timestamp = _diff_pres.timestamp; + _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); + _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celcius); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1243,6 +1246,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.temperature = -1000.0f; _diff_pres.voltage = voltage; /* announce the airspeed if needed, just publish else */ diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 5d94d4288..ff88b04c6 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,10 +54,11 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - float differential_pressure_pa; /**< Differential pressure reading */ - float max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 628977656..fa3367de9 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -105,6 +105,8 @@ struct sensor_combined_s { float differential_pressure_pa; /**< Airspeed sensor differential pressure */ uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ + float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + }; /** |