dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

[DO NOT MERGE] run sdp3x at lower rate (20 Hz sample rate)

This commit is contained in:
Daniel Agar 2021-09-20 11:03:29 -04:00
parent 1f8836f5a6
commit df8c9b4efb
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
2 changed files with 20 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/differential_pressure/sdp3x/SDP3X.cpp
View File

@ -77,7 +77,7 @@ SDP3X::configure()
int ret = write_command(SDP3X_CONT_MODE_STOP);
if (ret != PX4_OK) {
perf_count(_comms_errors);
perf_count(_configure_failed_perf);
DEVICE_DEBUG("stopping continous mode failed %d", ret);
if (ScheduledWorkItem::alone()) {
@ -159,10 +159,12 @@ int
SDP3X::collect()
{
perf_begin(_sample_perf);
perf_begin(_transfer_perf);
// read 6 bytes from the sensor
uint8_t val[6];
int ret = transfer(nullptr, 0, &val[0], sizeof(val));
perf_end(_transfer_perf);
if (ret != PX4_OK) {
perf_count(_comms_errors);
@ -178,7 +180,7 @@ SDP3X::collect()
int16_t P = (((int16_t)val[0]) << 8) | val[1];
int16_t temp = (((int16_t)val[3]) << 8) | val[4];
float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale);
float diff_press_pa_raw = static_cast<float>(P) / static_cast<float>(_scale) - _diff_pres_offset;
float temperature_c = temp / static_cast<float>(SDP3X_SCALE_TEMPERATURE);
if (PX4_ISFINITE(diff_press_pa_raw)) {
@ -186,8 +188,8 @@ SDP3X::collect()
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature_c;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset;
report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
report.differential_pressure_filtered_pa = diff_press_pa_raw;
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.device_id = _device_id.devid;
report.timestamp = hrt_absolute_time();
@ -202,6 +204,8 @@ SDP3X::collect()
void
SDP3X::RunImpl()
{
perf_count(_interval_perf);
switch (_state) {
case State::RequireConfig:
if (configure() == PX4_OK) {

19
src/drivers/differential_pressure/sdp3x/SDP3X.hpp
View File

@ -63,9 +63,7 @@
#define SDP3X_SCALE_PRESSURE_SDP32 240
#define SDP3X_SCALE_PRESSURE_SDP33 20
// Measurement rate is 20Hz
#define SPD3X_MEAS_RATE 100
#define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f
#define SPD3X_MEAS_RATE 20
#define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */
class SDP3X : public Airspeed, public I2CSPIDriver<SDP3X>
@ -76,11 +74,17 @@ public:
I2CSPIDriver(config),
_keep_retrying{config.keep_running}
{
_debug_enabled = true;
//_debug_enabled = true;
_retries = 2;
}
virtual ~SDP3X() = default;
virtual ~SDP3X()
{
perf_free(_configure_perf);
perf_free(_interval_perf);
perf_free(_transfer_perf);
perf_free(_configure_failed_perf);
};
static void print_usage();
@ -101,8 +105,6 @@ private:
int configure();
int read_scale();
math::LowPassFilter2p<float> _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ};
bool init_sdp3x();
/**
@ -120,4 +122,7 @@ private:
State _state{State::RequireConfig};
perf_counter_t _configure_perf{perf_alloc(PC_COUNT, MODULE_NAME": configure")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": configure")};
perf_counter_t _configure_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME": configure failed")};
};