Merged airspeed filtering from Thomas Gubler

This commit is contained in:
Lorenz Meier 2014-03-23 16:47:56 +01:00
commit e5b2281099
6 changed files with 18 additions and 5 deletions

View File

@ -79,6 +79,8 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <drivers/drv_airspeed.h> #include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@ -99,6 +101,8 @@
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */
/* Measurement rate is 100Hz */ /* Measurement rate is 100Hz */
#define MEAS_RATE 100.0f
#define MEAS_DRIVER_FILTER_FREQ 3.0f
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
class MEASAirspeed : public Airspeed class MEASAirspeed : public Airspeed
@ -116,6 +120,7 @@ protected:
virtual int measure(); virtual int measure();
virtual int collect(); virtual int collect();
math::LowPassFilter2p _filter;
}; };
/* /*
@ -124,7 +129,8 @@ protected:
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, 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.error_count = perf_event_count(_comms_errors);
report.temperature = temperature; report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa; report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
report.voltage = 0; report.voltage = 0;
report.max_differential_pressure_pa = _max_differential_pressure_pa; report.max_differential_pressure_pa = _max_differential_pressure_pa;

View File

@ -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_alt = buf.sensor.baro_alt_meter;
log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; 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 = buf.sensor.differential_pressure_pa;
log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa;
LOGBUFFER_WRITE_AND_COUNT(SENS); LOGBUFFER_WRITE_AND_COUNT(SENS);
} }

View File

@ -92,6 +92,7 @@ struct log_SENS_s {
float baro_alt; float baro_alt;
float baro_temp; float baro_temp;
float diff_pres; float diff_pres;
float diff_pres_filtered;
}; };
/* --- LPOS - LOCAL POSITION --- */ /* --- 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(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), 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(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),

View File

@ -1029,8 +1029,9 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_timestamp = _diff_pres.timestamp;
raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
_airspeed.timestamp = hrt_absolute_time(); _airspeed.timestamp = _diff_pres.timestamp;
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _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, _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); raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);

View File

@ -55,6 +55,7 @@ struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t error_count; uint64_t error_count;
float differential_pressure_pa; /**< 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 max_differential_pressure_pa; /**< Maximum differential pressure reading */
float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */
float temperature; /**< Temperature provided by sensor */ float temperature; /**< Temperature provided by sensor */

View File

@ -104,6 +104,8 @@ struct sensor_combined_s {
float differential_pressure_pa; /**< Airspeed sensor differential pressure */ float differential_pressure_pa; /**< Airspeed sensor differential pressure */
uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */
float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */
}; };
/** /**