From 28125aca3d7d110e3864757455175eacce1e5e67 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 22 Aug 2018 15:36:45 -0700 Subject: [PATCH] fxos8701cq:Clean up & paramater based low pass fileter Added paramater based low pass fileter settting from IMU_ACCEL_CUTOFF. removed gotos. --- src/drivers/imu/fxos8701cq/fxos8701cq.cpp | 26 +++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp index 0d50a0f243..cb6dfc8ca2 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp +++ b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp @@ -555,20 +555,33 @@ FXOS8701CQ::init() /* do SPI init (and probe) first */ if (SPI::init() != OK) { PX4_ERR("SPI init failed"); - goto out; + return PX4_ERROR; } /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) { - goto out; + return PX4_ERROR; } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) { - goto out; + return PX4_ERROR; + } + + // set software low pass filter for controllers + param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); + float accel_cut = FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; + + if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) { + PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut)); + + accel_set_driver_lowpass_filter(FXOS8701C_ACCEL_DEFAULT_RATE, accel_cut); + + } else { + PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); } reset(); @@ -578,7 +591,7 @@ FXOS8701CQ::init() if (ret != OK) { PX4_ERR("MAG init failed"); - goto out; + return PX4_ERROR; } /* fill report structures */ @@ -594,6 +607,7 @@ FXOS8701CQ::init() if (_mag->_mag_topic == nullptr) { PX4_ERR("ADVERT ERR"); + return PX4_ERROR; } @@ -609,10 +623,10 @@ FXOS8701CQ::init() if (_accel_topic == nullptr) { PX4_ERR("ADVERT ERR"); + return PX4_ERROR; } -out: - return ret; + return PX4_OK; }